Skip to content
Snippets Groups Projects

Compare revisions

Changes are shown as if the source revision was being merged into the target revision. Learn more about comparing revisions.

Source

Select target project
No results found
Select Git revision
  • master
  • simulated_grasping
  • v1.0
3 results

Target

Select target project
  • nikaviator/panda_simulation
1 result
Select Git revision
  • master
  • v1.0
2 results
Show changes
Commits on Source (28)
Showing
with 186 additions and 892 deletions
FROM osrf/ros:melodic-desktop-full-bionic
RUN apt-get update && apt-get install -q -y \
openssh-client
RUN echo 'source /opt/ros/melodic/setup.bash' >> /root/.bashrc
\ No newline at end of file
{
"name": "panda_simulation-dev",
"dockerFile": "Dockerfile",
"extensions": [
"ms-vscode.cpptools",
"ms-iot.vscode-ros"
],
"runArgs": [
"--cap-add=SYS_PTRACE",
"--security-opt",
"seccomp=unconfined",
"-v",
"${env:HOME}${env:USERPROFILE}/.ssh:/root/.ssh"
],
"settings": {
"terminal.integrated.shell.linux": "/bin/bash"
},
"postCreateCommand": "bash /catkin_ws/src/panda_simulation/scripts/docker-setup.sh",
"workspaceMount": "src=/Users/pekel/code/ros/panda_simulation,dst=/catkin_ws/src/panda_simulation,type=bind,consistency=cached",
"workspaceFolder": "/catkin_ws"
}
\ No newline at end of file
root = true
[*]
charset = utf-8
end_of_line = lf
indent_size = 2
indent_style = space
insert_final_newline = false
max_line_length = 120
tab_width = 2
ij_continuation_indent_size = 4
ij_formatter_off_tag = @formatter:off
ij_formatter_on_tag = @formatter:on
ij_formatter_tags_enabled = false
ij_smart_tabs = false
ij_wrap_on_typing = false
......@@ -30,15 +30,12 @@ find_package(catkin REQUIRED
# System dependencies are found with CMake's conventions
find_package(Boost REQUIRED COMPONENTS filesystem)
find_package(PkgConfig REQUIRED)
pkg_check_modules(JSONCPP jsoncpp)
message(${JSONCPP_LIBRARIES})
# ################################################################################################################################
# catkin specific configuration ##
# ################################################################################################################################
# The catkin_package macro generates cmake config files for your package Declare things to be passed to dependent projects
catkin_package(CATKIN_DEPENDS
catkin_package(
CATKIN_DEPENDS
moveit_core
moveit_visual_tools
moveit_ros_planning_interface
......@@ -56,18 +53,80 @@ catkin_package(CATKIN_DEPENDS
# Specify additional locations of header files Your package locations should be listed before other locations
include_directories(${catkin_INCLUDE_DIRS})
add_executable(robot_control_node src/robot_control_node.cpp)
add_executable(box_publisher_node src/box_publisher.cpp)
add_executable(robot_state_initializer_node src/robot_state_initializer.cpp)
add_executable(move_group_interface_tutorial src/move_group_interface_tutorial.cpp)
target_link_libraries(robot_control_node ${catkin_LIBRARIES} boost_filesystem ${JSONCPP_LIBRARIES})
target_link_libraries(box_publisher_node ${catkin_LIBRARIES})
target_link_libraries(robot_state_initializer_node ${catkin_LIBRARIES})
target_link_libraries(move_group_interface_tutorial ${catkin_LIBRARIES} ${Boost_LIBRARIES})
## Declare C++ executables
add_executable(${PROJECT_NAME}_robot_state_initializer_node src/robot_state_initializer.cpp)
# add custom controller as library
## Declare C++ libraries
add_library(${PROJECT_NAME}_controllers_lib src/joint_position_controller.cpp)
## Add cmake target dependencies of the library
## as an example, code may need to be generated before libraries
## either from message generation or dynamic reconfigure
## Rename C++ executable without prefix
## The above recommended prefix causes long target names, the following renames the
## target back to the shorter version for ease of user use
## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
set_target_properties(${PROJECT_NAME}_robot_state_initializer_node PROPERTIES OUTPUT_NAME robot_state_initializer_node PREFIX "")
# Specify libraries to link a library or executable target against
target_link_libraries(${PROJECT_NAME}_controllers_lib ${catkin_LIBRARIES})
target_link_libraries(${PROJECT_NAME}_robot_state_initializer_node ${catkin_LIBRARIES})
#############
## Install ##
#############
# all install targets should use catkin DESTINATION variables
# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
## Mark executable scripts (Python etc.) for installation
## in contrast to setup.py, you can choose the destination
# catkin_install_python(PROGRAMS
# scripts/my_python_script
# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
# )
## Mark executables for installation
## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html
# install(TARGETS ${PROJECT_NAME}_node
# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
# )
## Mark libraries for installation
## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html
# install(TARGETS ${PROJECT_NAME}
# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
# RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
# )
## Mark cpp header files for installation
# install(DIRECTORY include/${PROJECT_NAME}/
# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
# FILES_MATCHING PATTERN "*.h"
# PATTERN ".svn" EXCLUDE
# )
## Mark other files for installation (e.g. launch and bag files, etc.)
# install(FILES
# # myfile1
# # myfile2
# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
# )
#############
## Testing ##
#############
## Add gtest based cpp test target and link libraries
# catkin_add_gtest(${PROJECT_NAME}-test test/test_gazebo_zones.cpp)
# if(TARGET ${PROJECT_NAME}-test)
# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
# endif()
## Add folders to be run by python nosetests
# catkin_add_nosetests(test)
# panda_simulation
![Panda in Gazebo](assets/panda-in-gazebo.png?raw=true "Panda in Gazebo")
This package was written for ROS melodic running under Ubuntu 18.04. Run the following commands to make sure that all additional packages are installed:
```
mkdir -p catkin_ws/src
cd catkin_ws/src
git clone https://github.com/erdalpekel/panda_simulation.git
git clone https://github.com/erdalpekel/panda_moveit_config.git
git clone --branch simulation https://github.com/erdalpekel/franka_ros.git
cd ..
sudo apt-get install libboost-filesystem-dev
rosdep install --from-paths src --ignore-src -y --skip-keys libfranka
cd ..
```
It is also important that you build the *libfranka* library from source and pass its directory to *catkin_make* when building this ROS package as described in [this tutorial](https://frankaemika.github.io/docs/installation.html#building-from-source).
Currently it includes a controller parameter config file and a launch file to launch the [Gazebo](http://gazebosim.org) simulation environment and the Panda robot from FRANKA EMIKA in it with the necessary controllers.
Build the catkin workspace and run the simulation:
```
catkin_make -j4 -DCMAKE_BUILD_TYPE=Release -DFranka_DIR:PATH=/path/to/libfranka/build
source devel/setup.bash
roslaunch panda_simulation simulation.launch
```
Depending on your operating systems language you might need to export the numeric type so that rviz can read the floating point numbers in the robot model correctly:
```
export LC_NUMERIC="en_US.UTF-8"
```
Otherwise, the robot will appear in rviz in a collapsed state.
You can see the full explanation in my [blog post](https://erdalpekel.de/?p=55).
## Changelog:
[_MoveIt!_ constraint-aware planning](https://erdalpekel.de/?p=123)
This repository was extended with a ROS node that communicates with the _MoveIt!_ Planning Scene API. It makes sure that the motion planning pipeline avoids collision objects in the environment specified by the user in a separate directory (`~/.panda_simulation`) as _json_ files.
[Publishing a box at Panda's hand in _Gazebo_](https://erdalpekel.de/?p=123)
This repository was extended with a node that publishes a simple box object in the _Gazebo_ simulation at the hand of the robot. The position of this box will get updated as soon as the robot moves.
[Visual Studio Code Remote Docker](https://erdalpekel.de/?p=123)
I have added configuration files and additional setup scripts for developing and using this ROS package within a *Docker* container. Currently user interfaces for Gazebo and RViz are not supported.
[Position based trajectory execution](https://erdalpekel.de/?p=285)
The joint specifications in Gazebo were changed from an effort interface to position based interface. Furthermore, the PID controller was substituted with the simple gazebo internal position based control mechanism for a more stable movement profile of the robot. A custom joint position based controller was implemented in order to set the initial joint states of the robot to a valid configuration.
[Automatic robot state initialization](https://erdalpekel.de/?p=314)
A separate ROS node was implemented that starts a custom joint position controller and initializes the robot with a specific configuration. It switches back to the default controllers after the robot reaches the desired state.
![Panda state initialization in Gazebo](assets/robot-state-initializer.gif?raw=true "Panda state initialization in Gazebo")
\ No newline at end of file
assets/robot-state-initializer.gif

117 KiB

......@@ -3,8 +3,9 @@
type: joint_state_controller/JointStateController
publish_rate: 100
panda_arm_controller:
position_joint_trajectory_controller:
type: position_controllers/JointTrajectoryController
# type: effort_controllers/JointTrajectoryController
joints:
- panda_joint1
- panda_joint2
......@@ -17,15 +18,30 @@
constraints:
goal_time: 2.0
gains:
panda_joint1: { p: 12000, d: 50, i: 0.0, i_clamp: 10000 }
panda_joint2: { p: 30000, d: 100, i: 0.02, i_clamp: 10000 }
panda_joint3: { p: 18000, d: 50, i: 0.01, i_clamp: 1 }
panda_joint4: { p: 18000, d: 70, i: 0.01, i_clamp: 10000 }
panda_joint5: { p: 12000, d: 70, i: 0.01, i_clamp: 1 }
panda_joint6: { p: 7000, d: 50, i: 0.01, i_clamp: 1 }
panda_joint7: { p: 2000, d: 20, i: 0.0, i_clamp: 1 }
state_publish_rate: 100
panda_hand_controller:
type: position_controllers/JointTrajectoryController
# type: position_controllers/JointTrajectoryController
type: effort_controllers/JointTrajectoryController
joints:
- panda_finger_joint1
- panda_finger_joint2
gains:
panda_finger_joint1: { p: 5, d: 3.0, i: 0, i_clamp: 1 }
panda_finger_joint2: { p: 5, d: 1.0, i: 0, i_clamp: 1 }
state_publish_rate: 25
joint_position_controller:
type: panda_simulation/JointPositionController
arm_id: panda
......
<launch>
<param name="box_description" command="$(find xacro)/xacro $(find panda_simulation)/models/box.xacro"/>
<node name="box_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" args="robot_description:=box_description" />
<node name="spawn_object" pkg="gazebo_ros" type="spawn_model" args="-param box_description -urdf -model box"/>
<!-- launch node -->
<node pkg="panda_simulation" type="box_publisher_node" name="box_publisher_node" />
</launch>
\ No newline at end of file
<launch>
<param name="robot_description" command="$(find xacro)/xacro $(find franka_description)/robots/panda_arm_hand.urdf.xacro" />
<!-- GAZEBO arguments -->
<arg name="paused" default="false" />
<arg name="use_sim_time" default="true" />
<arg name="gui" default="true" />
<arg name="headless" default="false" />
<arg name="debug" default="false" />
<arg name="load_gripper" default="true" />
<!--launch GAZEBO with own world configuration -->
<include file="$(find gazebo_ros)/launch/empty_world.launch">
<!-- <arg name="world_name" value="$(find robotics_assisted_tomography)/worlds/tomography.world"/> -->
<arg name="debug" value="$(arg debug)" />
<arg name="gui" value="$(arg gui)" />
<arg name="paused" value="$(arg paused)" />
<arg name="use_sim_time" value="$(arg use_sim_time)" />
<arg name="headless" value="$(arg headless)" />
</include>
<node name="rqt_console" pkg="rqt_console" type="rqt_console" />
<!-- <node name="rqt_logger_level" pkg="rqt_logger_level" type="rqt_logger_level" /> -->
<node name="spawn_urdf" pkg="gazebo_ros" type="spawn_model" args="-param robot_description -urdf -model panda" />
<!-- Load joint controller configurations from YAML file to parameter server -->
<rosparam file="$(find panda_simulation)/config/panda_control.yaml" command="load" />
<!-- load the controllers -->
<node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen" args="joint_state_controller panda_arm_controller" />
<node if="$(arg load_gripper)" name="controller_spawner_hand" pkg="controller_manager" type="spawner" respawn="false" output="screen" args="panda_hand_controller" />
<!-- convert joint states to TF transforms for rviz, etc -->
<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" respawn="true" output="screen" />
<include file="$(find panda_moveit_config)/launch/planning_context.launch">
<arg name="load_robot_description" value="true" />
<arg name="load_gripper" value="$(arg load_gripper)" />
</include>
<include file="$(find panda_moveit_config)/launch/move_group.launch">
<arg name="load_gripper" value="$(arg load_gripper)" />
</include>
<group if="$(arg gui)">
<include file="$(find panda_moveit_config)/launch/moveit_rviz.launch" />
</group>
<node name="joint_state_desired_publisher" pkg="topic_tools" type="relay" args="joint_states joint_states_desired" />
<!-- launch robot control node for moveit motion planning -->
<node pkg="panda_simulation" type="robot_control_node" name="robot_control_node" output="screen" />
<!-- load (not start!) custom joint position controller -->
<node pkg="controller_manager" type="spawner" name="joint_position_launcher" args="--stopped joint_position_controller" />
<!-- run custom node for automatic intialization -->
<node pkg="panda_simulation" type="robot_state_initializer_node" name="robot_state_initializer_node" />
<node pkg="panda_simulation" type="move_group_interface_tutorial" name="move_group_interface_tutorial" respawn="false" output="screen">
</node>
</launch>
<launch>
<param name="robot_description" command="$(find xacro)/xacro $(find franka_description)/robots/panda_arm_hand.urdf.xacro" />
<!-- GAZEBO arguments -->
<arg name="paused" default="false" />
......@@ -8,10 +7,16 @@
<arg name="headless" default="false" />
<arg name="debug" default="false" />
<arg name="load_gripper" default="true" />
<arg name="slow_simulation" default="false" />
<!-- don't include franka_control, but include the controller stuff from there -->
<param name="robot_description" command="$(find xacro)/xacro '$(find franka_description)/robots/panda_arm_hand.urdf.xacro'" if="$(arg load_gripper)" />
<param name="robot_description" command="$(find xacro)/xacro '$(find franka_description)/robots/panda_arm.urdf.xacro'" unless="$(arg load_gripper)" />
<!--launch GAZEBO with own world configuration -->
<include file="$(find gazebo_ros)/launch/empty_world.launch">
<!-- <arg name="world_name" value="$(find robotics_assisted_tomography)/worlds/tomography.world"/> -->
<arg name="world_name" if="$(arg slow_simulation)" value="$(find panda_simulation)/worlds/simulator.world"/>
<arg name="debug" value="$(arg debug)" />
<arg name="gui" value="$(arg gui)" />
<arg name="paused" value="$(arg paused)" />
......@@ -19,17 +24,14 @@
<arg name="headless" value="$(arg headless)" />
</include>
<node name="rqt_console" pkg="rqt_console" type="rqt_console" />
<!-- <node name="rqt_logger_level" pkg="rqt_logger_level" type="rqt_logger_level" /> -->
<node name="spawn_urdf" pkg="gazebo_ros" type="spawn_model" args="-param robot_description -urdf -model panda" />
<!-- Load joint controller configurations from YAML file to parameter server -->
<rosparam file="$(find panda_simulation)/config/panda_control.yaml" command="load" />
<!-- load the controllers -->
<node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen" args="joint_state_controller panda_arm_controller" />
<node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen" args="joint_state_controller position_joint_trajectory_controller" />
<node if="$(arg load_gripper)" name="controller_spawner_hand" pkg="controller_manager" type="spawner" respawn="false" output="screen" args="panda_hand_controller" />
......@@ -43,15 +45,9 @@
<include file="$(find panda_moveit_config)/launch/move_group.launch">
<arg name="load_gripper" value="$(arg load_gripper)" />
</include>
<group if="$(arg gui)">
<include file="$(find panda_moveit_config)/launch/moveit_rviz.launch" />
</group>
<node name="joint_state_desired_publisher" pkg="topic_tools" type="relay" args="joint_states joint_states_desired" />
<!-- launch robot control node for moveit motion planning -->
<node pkg="panda_simulation" type="robot_control_node" name="robot_control_node" output="screen" />
<!-- load (not start!) custom joint position controller -->
<node pkg="controller_manager" type="spawner" name="joint_position_launcher" args="--stopped joint_position_controller" />
......
/**
\mainpage
\htmlinclude manifest.html
<!--
Provide an overview of your package.
-->
*/
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="simple_box">
<link name="object_base_link">
</link>
<joint name="object_base_joint" type="fixed">
<parent link="object_base_link"/>
<child link="object_link"/>
<axis xyz="0 0 1" />
<origin xyz="0 0 0" rpy="0 0 0"/>
</joint>
<link name="object_link">
<inertial>
<origin xyz="0 0 0" />
<mass value="1.0" />
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="100.0" iyz="0.0" izz="1.0" />
</inertial>
<visual>
<origin xyz="0 0 0"/>
<geometry>
<box size="0.04 0.04 0.08" />
</geometry>
</visual>
<collision>
<origin xyz="0 0 0"/>
<geometry>
<box size="0.04 0.04 0.08" />
</geometry>
</collision>
</link>
<gazebo reference="object_base_link">
<gravity>0</gravity>
</gazebo>
<gazebo reference="object_link">
<material>Gazebo/Blue</material>
</gazebo>
</robot>
\ No newline at end of file
<?xml version="1.0"?>
<package format="2">
<name>panda_simulation</name>
<version>0.0.0</version>
<version>0.2.0</version>
<description>The panda_simulation package</description>
<!-- One maintainer tag required, multiple allowed, one person per tag -->
<!-- Example: -->
<!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
<maintainer email="erdal@todo.todo">erdal</maintainer>
<maintainer email="johannes.mey@tu-dresden.de">Johannes Mey</maintainer>
<maintainer email="sebastian.ebert@tu-dresden.de">Sebastian Ebert</maintainer>
<!-- Url tags are optional, but multiple are allowed, one per tag -->
<!-- Optional attribute type can be: website, bugtracker, or repository -->
<!-- Example: -->
<url type="website">http://wiki.ros.org/gazebo_zones</url>
<!-- Author tags are optional, multiple are allowed, one per tag -->
<!-- Authors do not have to be maintainers, but could be -->
<!-- Example: -->
<!-- <author email="jane.doe@example.com">Jane Doe</author> -->
<author email="johannes.mey@tu-dresden.de">Johannes Mey</author>
<author email="sebastian.ebert@tu-dresden.de">Sebastian Ebert</author>
<author>Erdal Pekel</author>
<!-- One license tag required, multiple allowed, one license per tag -->
<!-- Commonly used license strings: -->
<!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
<license>TODO</license>
<license>MIT</license>
<buildtool_depend>catkin</buildtool_depend>
<depend>controller_manager</depend>
<depend>effort_controllers</depend>
<depend>gazebo_ros</depend>
<!-- <depend>gazebo_ros</depend>-->
<depend>gazebo_plugins</depend>
<depend>gazebo_ros_control</depend>
<depend>joint_state_controller</depend>
<depend>joint_state_publisher</depend>
......@@ -30,7 +45,6 @@
<depend>std_msgs</depend>
<depend>tf</depend>
<depend>xacro</depend>
<depend>libjsoncpp-dev</depend>
<depend>moveit_simple_controller_manager</depend>
<!-- CUSTOM -->
......@@ -41,10 +55,17 @@
<depend>controller_interface</depend>
<depend>hardware_interface</depend>
<build_depend>sdformat</build_depend>
<exec_depend>sdformat</exec_depend>
<exec_depend>gazebo_ros</exec_depend>
<exec_depend>pluginlib</exec_depend>
<!-- The export tag contains other, unspecified, tags -->
<export>
<controller_interface plugin="${prefix}/controller_plugins.xml" />
<gazebo_ros gazebo_media_path="/usr/share/gazebo-9/media/materials/scripts"/>
</export>
</package>
- builder: doxygen
name: C++ API
file_patterns: '*.c *.cpp *.h *.cc *.hh *.dox'
homepage: https://st.inf.tu-dresden.de/ceti-robots
#!/bin/bash
REPOSITORY_DIR=$(pwd)
WORKSPACE_DIR=/catkin_ws
DEPENDENCIES_DIR=$WORKSPACE_DIR/dependencies
APP_DIR=/root/.panda_simulation
# Install libfranka
mkdir $DEPENDENCIES_DIR && cd $DEPENDENCIES_DIR
git clone --recursive https://github.com/frankaemika/libfranka
cd libfranka
git checkout 0.5.0
git submodule update
mkdir build && cd build
cmake -DCMAKE_BUILD_TYPE=Release -DBUILD_TESTS=0 ..
cmake --build . -- -j$(nproc)
# Clone the franka_ros and panda_moveit_config repositories for simulating Panda robot
cd $WORKSPACE_DIR/src
git clone https://github.com/erdalpekel/panda_moveit_config.git
git clone --branch simulation https://github.com/erdalpekel/franka_ros.git
# Install package dependencies with rosdep
cd $WORKSPACE_DIR
rosdep install --from-paths src --ignore-src -y --skip-keys libfranka --skip-keys moveit_perception
# Clone the Visual-Studio-Code-ROS repository into the workspace directory /catkin_ws
cd $WORKSPACE_DIR
git clone git@github.com:erdalpekel/Visual-Studio-Code-ROS.git
mv Visual-Studio-Code-ROS .vscode
# create app directory for config files
mkdir $APP_DIR
\ No newline at end of file
#include <gazebo_msgs/SetModelState.h>
#include <moveit/move_group_interface/move_group_interface.h>
#include <moveit/robot_model_loader/robot_model_loader.h>
#include <ros/ros.h>
#include <sensor_msgs/JointState.h>
static const std::string PLANNING_GROUP_ARM = "panda_arm";
static const double PANDA_ARM_TO_HAND_OFFSET = 0.12;
static const double PANDA_HAND_TO_FINGER_OFFSET = 0.04;
ros::Publisher gazebo_model_state_pub;
robot_model::RobotModelPtr kinematic_model;
robot_state::RobotStatePtr kinematic_state;
void jointStatesCallback(const sensor_msgs::JointState &joint_states_current)
{
const robot_state::JointModelGroup *joint_model_group = kinematic_model->getJointModelGroup("panda_arm");
const std::vector<std::string> &joint_names = joint_model_group->getJointModelNames();
std::vector<double> joint_states;
for (size_t i = 0; i < joint_states_current.position.size() - 2; ++i)
{
joint_states.push_back(joint_states_current.position[i + 2]);
}
kinematic_state->setJointGroupPositions(joint_model_group, joint_states);
const Eigen::Affine3d &end_effector_state = kinematic_state->getGlobalLinkTransform("panda_link8");
double end_effector_z_offset = PANDA_ARM_TO_HAND_OFFSET + PANDA_HAND_TO_FINGER_OFFSET;
Eigen::Affine3d tmp_transform(Eigen::Translation3d(Eigen::Vector3d(0.0, 0.0, end_effector_z_offset)));
Eigen::Affine3d newState = end_effector_state * tmp_transform;
geometry_msgs::Pose pose;
pose.position.x = newState.translation().x();
pose.position.y = newState.translation().y();
pose.position.z = newState.translation().z();
Eigen::Quaterniond quat(newState.rotation());
pose.orientation.w = quat.w();
pose.orientation.x = quat.x();
pose.orientation.y = quat.y();
pose.orientation.z = quat.z();
ROS_DEBUG_STREAM("translation" << std::endl << newState.translation());
ROS_DEBUG_STREAM("rotation" << std::endl << newState.rotation());
gazebo_msgs::ModelState model_state;
// This string results from the spawn_urdf call in the box.launch file argument: -model box
model_state.model_name = std::string("box");
model_state.pose = pose;
model_state.reference_frame = std::string("world");
gazebo_model_state_pub.publish(model_state);
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "box_publisher_node");
ros::NodeHandle node_handle;
ros::Subscriber joint_states_sub = node_handle.subscribe("/joint_states", 1, jointStatesCallback);
robot_model_loader::RobotModelLoader robot_model_loader("robot_description");
kinematic_model = robot_model_loader.getModel();
kinematic_state = robot_state::RobotStatePtr(new robot_state::RobotState(kinematic_model));
gazebo_model_state_pub = node_handle.advertise<gazebo_msgs::ModelState>("/gazebo/set_model_state", 1);
ros::spin();
return 0;
}
\ No newline at end of file
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2013, SRI International
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of SRI International nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
/* Author: Sachin Chitta, Dave Coleman, Mike Lautman */
#include <moveit/move_group_interface/move_group_interface.h>
#include <moveit/planning_scene_interface/planning_scene_interface.h>
#include <moveit_msgs/DisplayRobotState.h>
#include <moveit_msgs/DisplayTrajectory.h>
#include <moveit_msgs/AttachedCollisionObject.h>
#include <moveit_msgs/CollisionObject.h>
#include <moveit_visual_tools/moveit_visual_tools.h>
int main(int argc, char** argv)
{
ros::init(argc, argv, "move_group_interface_tutorial");
ros::NodeHandle node_handle;
ros::AsyncSpinner spinner(1);
spinner.start();
// BEGIN_TUTORIAL
//
// Setup
// ^^^^^
//
// MoveIt operates on sets of joints called "planning groups" and stores them in an object called
// the `JointModelGroup`. Throughout MoveIt the terms "planning group" and "joint model group"
// are used interchangably.
static const std::string PLANNING_GROUP = "panda_arm";
// The :planning_interface:`MoveGroupInterface` class can be easily
// setup using just the name of the planning group you would like to control and plan for.
moveit::planning_interface::MoveGroupInterface move_group(PLANNING_GROUP);
// We will use the :planning_interface:`PlanningSceneInterface`
// class to add and remove collision objects in our "virtual world" scene
moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
// Raw pointers are frequently used to refer to the planning group for improved performance.
const robot_state::JointModelGroup* joint_model_group =
move_group.getCurrentState()->getJointModelGroup(PLANNING_GROUP);
// Visualization
// ^^^^^^^^^^^^^
//
// The package MoveItVisualTools provides many capabilities for visualizing objects, robots,
// and trajectories in RViz as well as debugging tools such as step-by-step introspection of a script.
namespace rvt = rviz_visual_tools;
moveit_visual_tools::MoveItVisualTools visual_tools("panda_link0");
visual_tools.deleteAllMarkers();
// Remote control is an introspection tool that allows users to step through a high level script
// via buttons and keyboard shortcuts in RViz
visual_tools.loadRemoteControl();
// RViz provides many types of markers, in this demo we will use text, cylinders, and spheres
Eigen::Isometry3d text_pose = Eigen::Isometry3d::Identity();
text_pose.translation().z() = 1.75;
visual_tools.publishText(text_pose, "MoveGroupInterface Demo", rvt::WHITE, rvt::XLARGE);
// Batch publishing is used to reduce the number of messages being sent to RViz for large visualizations
visual_tools.trigger();
// Getting Basic Information
// ^^^^^^^^^^^^^^^^^^^^^^^^^
//
// We can print the name of the reference frame for this robot.
ROS_INFO_NAMED("tutorial", "Planning frame: %s", move_group.getPlanningFrame().c_str());
// We can also print the name of the end-effector link for this group.
ROS_INFO_NAMED("tutorial", "End effector link: %s", move_group.getEndEffectorLink().c_str());
// We can get a list of all the groups in the robot:
ROS_INFO_NAMED("tutorial", "Available Planning Groups:");
std::copy(move_group.getJointModelGroupNames().begin(), move_group.getJointModelGroupNames().end(),
std::ostream_iterator<std::string>(std::cout, ", "));
// Start the demo
// ^^^^^^^^^^^^^^^^^^^^^^^^^
visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to start the demo");
// Planning to a Pose goal
// ^^^^^^^^^^^^^^^^^^^^^^^
// We can plan a motion for this group to a desired pose for the
// end-effector.
geometry_msgs::Pose target_pose1;
target_pose1.orientation.w = 1.0;
target_pose1.position.x = 0.28;
target_pose1.position.y = -0.2;
target_pose1.position.z = 0.5;
move_group.setPoseTarget(target_pose1);
// Now, we call the planner to compute the plan and visualize it.
// Note that we are just planning, not asking move_group
// to actually move the robot.
moveit::planning_interface::MoveGroupInterface::Plan my_plan;
bool success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
ROS_INFO_NAMED("tutorial", "Visualizing plan 1 (pose goal) %s", success ? "" : "FAILED");
// Visualizing plans
// ^^^^^^^^^^^^^^^^^
// We can also visualize the plan as a line with markers in RViz.
ROS_INFO_NAMED("tutorial", "Visualizing plan 1 as trajectory line");
visual_tools.publishAxisLabeled(target_pose1, "pose1");
visual_tools.publishText(text_pose, "Pose Goal", rvt::WHITE, rvt::XLARGE);
visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group);
visual_tools.trigger();
visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo");
// Moving to a pose goal
// ^^^^^^^^^^^^^^^^^^^^^
//
// Moving to a pose goal is similar to the step above
// except we now use the move() function. Note that
// the pose goal we had set earlier is still active
// and so the robot will try to move to that goal. We will
// not use that function in this tutorial since it is
// a blocking function and requires a controller to be active
// and report success on execution of a trajectory.
/* Uncomment below line when working with a real robot */
move_group.move();
// Planning to a joint-space goal
// ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
//
// Let's set a joint space goal and move towards it. This will replace the
// pose target we set above.
//
// To start, we'll create an pointer that references the current robot's state.
// RobotState is the object that contains all the current position/velocity/acceleration data.
moveit::core::RobotStatePtr current_state = move_group.getCurrentState();
//
// Next get the current set of joint values for the group.
std::vector<double> joint_group_positions;
current_state->copyJointGroupPositions(joint_model_group, joint_group_positions);
// Now, let's modify one of the joints, plan to the new joint space goal and visualize the plan.
joint_group_positions[0] = -1.0; // radians
move_group.setJointValueTarget(joint_group_positions);
// We lower the allowed maximum velocity and acceleration to 5% of their maximum.
// The default values are 10% (0.1).
// Set your preferred defaults in the joint_limits.yaml file of your robot's moveit_config
// or set explicit factors in your code if you need your robot to move faster.
move_group.setMaxVelocityScalingFactor(0.05);
move_group.setMaxAccelerationScalingFactor(0.05);
success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
ROS_INFO_NAMED("tutorial", "Visualizing plan 2 (joint space goal) %s", success ? "" : "FAILED");
// Visualize the plan in RViz
visual_tools.deleteAllMarkers();
visual_tools.publishText(text_pose, "Joint Space Goal", rvt::WHITE, rvt::XLARGE);
visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group);
visual_tools.trigger();
visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo");
// Planning with Path Constraints
// ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
//
// Path constraints can easily be specified for a link on the robot.
// Let's specify a path constraint and a pose goal for our group.
// First define the path constraint.
moveit_msgs::OrientationConstraint ocm;
ocm.link_name = "panda_link7";
ocm.header.frame_id = "panda_link0";
ocm.orientation.w = 1.0;
ocm.absolute_x_axis_tolerance = 0.1;
ocm.absolute_y_axis_tolerance = 0.1;
ocm.absolute_z_axis_tolerance = 0.1;
ocm.weight = 1.0;
// Now, set it as the path constraint for the group.
moveit_msgs::Constraints test_constraints;
test_constraints.orientation_constraints.push_back(ocm);
move_group.setPathConstraints(test_constraints);
// We will reuse the old goal that we had and plan to it.
// Note that this will only work if the current state already
// satisfies the path constraints. So we need to set the start
// state to a new pose.
robot_state::RobotState start_state(*move_group.getCurrentState());
geometry_msgs::Pose start_pose2;
start_pose2.orientation.w = 1.0;
start_pose2.position.x = 0.55;
start_pose2.position.y = -0.05;
start_pose2.position.z = 0.8;
start_state.setFromIK(joint_model_group, start_pose2);
move_group.setStartState(start_state);
// Now we will plan to the earlier pose target from the new
// start state that we have just created.
move_group.setPoseTarget(target_pose1);
// Planning with constraints can be slow because every sample must call an inverse kinematics solver.
// Lets increase the planning time from the default 5 seconds to be sure the planner has enough time to succeed.
move_group.setPlanningTime(10.0);
success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
ROS_INFO_NAMED("tutorial", "Visualizing plan 3 (constraints) %s", success ? "" : "FAILED");
// Visualize the plan in RViz
visual_tools.deleteAllMarkers();
visual_tools.publishAxisLabeled(start_pose2, "start");
visual_tools.publishAxisLabeled(target_pose1, "goal");
visual_tools.publishText(text_pose, "Constrained Goal", rvt::WHITE, rvt::XLARGE);
visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group);
visual_tools.trigger();
visual_tools.prompt("next step");
// When done with the path constraint be sure to clear it.
move_group.clearPathConstraints();
// Cartesian Paths
// ^^^^^^^^^^^^^^^
// You can plan a Cartesian path directly by specifying a list of waypoints
// for the end-effector to go through. Note that we are starting
// from the new start state above. The initial pose (start state) does not
// need to be added to the waypoint list but adding it can help with visualizations
std::vector<geometry_msgs::Pose> waypoints;
waypoints.push_back(start_pose2);
geometry_msgs::Pose target_pose3 = start_pose2;
target_pose3.position.z -= 0.2;
waypoints.push_back(target_pose3); // down
target_pose3.position.y -= 0.2;
waypoints.push_back(target_pose3); // right
target_pose3.position.z += 0.2;
target_pose3.position.y += 0.2;
target_pose3.position.x -= 0.2;
waypoints.push_back(target_pose3); // up and left
// We want the Cartesian path to be interpolated at a resolution of 1 cm
// which is why we will specify 0.01 as the max step in Cartesian
// translation. We will specify the jump threshold as 0.0, effectively disabling it.
// Warning - disabling the jump threshold while operating real hardware can cause
// large unpredictable motions of redundant joints and could be a safety issue
moveit_msgs::RobotTrajectory trajectory;
const double jump_threshold = 0.0;
const double eef_step = 0.01;
double fraction = move_group.computeCartesianPath(waypoints, eef_step, jump_threshold, trajectory);
ROS_INFO_NAMED("tutorial", "Visualizing plan 4 (Cartesian path) (%.2f%% acheived)", fraction * 100.0);
// Visualize the plan in RViz
visual_tools.deleteAllMarkers();
visual_tools.publishText(text_pose, "Cartesian Path", rvt::WHITE, rvt::XLARGE);
visual_tools.publishPath(waypoints, rvt::LIME_GREEN, rvt::SMALL);
for (std::size_t i = 0; i < waypoints.size(); ++i)
visual_tools.publishAxisLabeled(waypoints[i], "pt" + std::to_string(i), rvt::SMALL);
visual_tools.trigger();
visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo");
// Cartesian motions should often be slow, e.g. when approaching objects. The speed of cartesian
// plans cannot currently be set through the maxVelocityScalingFactor, but requires you to time
// the trajectory manually, as described [here](https://groups.google.com/forum/#!topic/moveit-users/MOoFxy2exT4).
// Pull requests are welcome.
// You can execute a trajectory by wrapping it in a plan like this.
moveit::planning_interface::MoveGroupInterface::Plan cartesian_plan;
cartesian_plan.trajectory_ = trajectory;
move_group.execute(cartesian_plan);
// Adding/Removing Objects and Attaching/Detaching Objects
// ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
//
// Define a collision object ROS message.
moveit_msgs::CollisionObject collision_object;
collision_object.header.frame_id = move_group.getPlanningFrame();
// The id of the object is used to identify it.
collision_object.id = "box1";
// Define a box to add to the world.
shape_msgs::SolidPrimitive primitive;
primitive.type = primitive.BOX;
primitive.dimensions.resize(3);
primitive.dimensions[0] = 0.4;
primitive.dimensions[1] = 0.1;
primitive.dimensions[2] = 0.4;
// Define a pose for the box (specified relative to frame_id)
geometry_msgs::Pose box_pose;
box_pose.orientation.w = 1.0;
box_pose.position.x = 0.4;
box_pose.position.y = -0.2;
box_pose.position.z = 1.0;
collision_object.primitives.push_back(primitive);
collision_object.primitive_poses.push_back(box_pose);
collision_object.operation = collision_object.ADD;
std::vector<moveit_msgs::CollisionObject> collision_objects;
collision_objects.push_back(collision_object);
// Now, let's add the collision object into the world
ROS_INFO_NAMED("tutorial", "Add an object into the world");
planning_scene_interface.addCollisionObjects(collision_objects);
// Show text in RViz of status
visual_tools.publishText(text_pose, "Add object", rvt::WHITE, rvt::XLARGE);
visual_tools.trigger();
// Wait for MoveGroup to receive and process the collision object message
visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to once the collision object appears in RViz");
// Now when we plan a trajectory it will avoid the obstacle
move_group.setStartState(*move_group.getCurrentState());
geometry_msgs::Pose another_pose;
another_pose.orientation.w = 1.0;
another_pose.position.x = 0.4;
another_pose.position.y = -0.4;
another_pose.position.z = 0.9;
move_group.setPoseTarget(another_pose);
success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
ROS_INFO_NAMED("tutorial", "Visualizing plan 5 (pose goal move around cuboid) %s", success ? "" : "FAILED");
// Visualize the plan in RViz
visual_tools.deleteAllMarkers();
visual_tools.publishText(text_pose, "Obstacle Goal", rvt::WHITE, rvt::XLARGE);
visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group);
visual_tools.trigger();
visual_tools.prompt("next step");
// Now, let's attach the collision object to the robot.
ROS_INFO_NAMED("tutorial", "Attach the object to the robot");
move_group.attachObject(collision_object.id);
// Show text in RViz of status
visual_tools.publishText(text_pose, "Object attached to robot", rvt::WHITE, rvt::XLARGE);
visual_tools.trigger();
/* Wait for MoveGroup to receive and process the attached collision object message */
visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to once the collision object attaches to the "
"robot");
// Now, let's detach the collision object from the robot.
ROS_INFO_NAMED("tutorial", "Detach the object from the robot");
move_group.detachObject(collision_object.id);
// Show text in RViz of status
visual_tools.publishText(text_pose, "Object dettached from robot", rvt::WHITE, rvt::XLARGE);
visual_tools.trigger();
/* Wait for MoveGroup to receive and process the attached collision object message */
visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to once the collision object detaches to the "
"robot");
// Now, let's remove the collision object from the world.
ROS_INFO_NAMED("tutorial", "Remove the object from the world");
std::vector<std::string> object_ids;
object_ids.push_back(collision_object.id);
planning_scene_interface.removeCollisionObjects(object_ids);
// Show text in RViz of status
visual_tools.publishText(text_pose, "Object removed", rvt::WHITE, rvt::XLARGE);
visual_tools.trigger();
/* Wait for MoveGroup to receive and process the attached collision object message */
visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to once the collision object disapears");
// END_TUTORIAL
ros::shutdown();
return 0;
}
#include <jsoncpp/json/json.h>
#include <moveit/move_group_interface/move_group_interface.h>
#include <moveit/planning_scene/planning_scene.h>
#include <moveit/planning_scene_interface/planning_scene_interface.h>
#include <moveit_visual_tools/moveit_visual_tools.h>
#include <ros/ros.h>
#include <boost/filesystem.hpp>
static const std::string PLANNING_GROUP_ARM = "panda_arm";
static const std::string APP_DIRECTORY_NAME = ".panda_simulation";
moveit_msgs::CollisionObject extractObstacleFromJson(Json::Value &root, std::string name)
{
moveit_msgs::CollisionObject collision_object;
collision_object.header.frame_id = "world";
collision_object.id = name;
const Json::Value dimensions = root["dimensions"];
ROS_INFO_STREAM("Extracted dimensions: " << dimensions);
// Define a box to add to the world.
shape_msgs::SolidPrimitive primitive;
primitive.type = primitive.BOX;
primitive.dimensions.resize(3);
primitive.dimensions[0] = dimensions["x"].asDouble();
primitive.dimensions[1] = dimensions["y"].asDouble();
primitive.dimensions[2] = dimensions["z"].asDouble();
const Json::Value position = root["position"];
ROS_INFO_STREAM("Extracted position: " << position);
const Json::Value orientation = root["orientation"];
ROS_INFO_STREAM("Extracted orientation: " << orientation);
// Define a pose for the box (specified relative to frame_id)
geometry_msgs::Pose box_pose;
box_pose.orientation.w = orientation["w"].asDouble();
box_pose.orientation.x = orientation["x"].asDouble();
box_pose.orientation.y = orientation["y"].asDouble();
box_pose.orientation.z = orientation["z"].asDouble();
// MoveIt! planning scene expects the center of the object as position.
// We add half of its dimension to its position
box_pose.position.x = position["x"].asDouble() + primitive.dimensions[0] / 2.0;
box_pose.position.y = position["y"].asDouble() + primitive.dimensions[1] / 2.0;
box_pose.position.z = position["z"].asDouble() + primitive.dimensions[2] / 2.0;
collision_object.primitives.push_back(primitive);
collision_object.primitive_poses.push_back(box_pose);
collision_object.operation = collision_object.ADD;
return std::move(collision_object);
}
int main(int argc, char **argv)
{
namespace fs = boost::filesystem;
ROS_INFO("RUNNING robot_control_node");
ros::init(argc, argv, "robot_control_node");
ros::NodeHandle node_handle;
ros::AsyncSpinner spinner(1);
spinner.start();
moveit::planning_interface::MoveGroupInterface move_group_arm(PLANNING_GROUP_ARM);
ros::Publisher planning_scene_diff_publisher = node_handle.advertise<moveit_msgs::PlanningScene>("planning_scene", 1);
ros::WallDuration sleep_t(0.5);
while (planning_scene_diff_publisher.getNumSubscribers() < 1)
{
sleep_t.sleep();
}
moveit_msgs::PlanningScene planning_scene;
// read JSON files from ~/.panda_simulation
fs::path home(getenv("HOME"));
if (fs::is_directory(home))
{
fs::path app_directory(home);
app_directory /= APP_DIRECTORY_NAME;
if (!fs::exists(app_directory) && !fs::is_directory(app_directory))
{
ROS_WARN_STREAM(app_directory << " does not exist");
// Create .panda_simulation directory
std::string path(getenv("HOME"));
path += "/.panda_simulation";
ROS_INFO("Creating %s collision objects directory.", path);
try
{
boost::filesystem::create_directory(path);
}
catch (const std::exception&)
{
ROS_ERROR(
"%s directory could not be created."
"Please create this directory yourself "
"if you want to specify collision objects.", path.c_str());
return -1;
}
}
std::vector<moveit_msgs::CollisionObject> collision_objects;
ROS_INFO_STREAM(app_directory << " is a directory containing:");
for (auto &entry : boost::make_iterator_range(fs::directory_iterator(app_directory), {}))
{
ROS_INFO_STREAM(entry);
std::ifstream file_stream(entry.path().string(), std::ifstream::binary);
if (file_stream)
{
Json::Value root;
file_stream >> root;
moveit_msgs::CollisionObject collision_object = extractObstacleFromJson(root, entry.path().stem().string());
collision_objects.push_back(collision_object);
}
else
{
ROS_WARN_STREAM("could not open file " << entry.path());
}
}
// Publish the collision objects to the scene
for (const auto &collision_object : collision_objects)
{
collision_object.header.frame_id = move_group_arm.getPlanningFrame();
planning_scene.world.collision_objects.push_back(collision_object);
}
ROS_INFO_STREAM("# collision objects " << planning_scene.world.collision_objects.size());
planning_scene.is_diff = true;
planning_scene_diff_publisher.publish(planning_scene);
ROS_INFO("robot_control_node is ready");
ros::waitForShutdown();
return 0;
}
}
\ No newline at end of file
......@@ -20,7 +20,7 @@ int main(int argc, char **argv) {
ros::Duration(2.0).sleep();
// 1. switch from default joint trajectory controller to custom position controller
std::string panda_arm_controller{"panda_arm_controller"}, panda_hand_controller{"panda_hand_controller"},
std::string panda_arm_controller{"position_joint_trajectory_controller"}, panda_hand_controller{"panda_hand_controller"},
joint_position_controller{"joint_position_controller"};
std::vector<std::string> stop_controllers{panda_arm_controller, panda_hand_controller};
std::vector<std::string> start_controllers{joint_position_controller};
......@@ -62,11 +62,11 @@ int main(int argc, char **argv) {
srv_switch_controller.request.start_controllers = start_controllers;
if (switch_controller_client.call(srv_switch_controller)) {
ROS_INFO_STREAM("Success switching controllers from " << stream_stop_controllers.str() << " to "
<< stream_start_controllers.str());
ROS_INFO_STREAM("Success switching controllers from " << stream_start_controllers.str() << " to "
<< stream_stop_controllers.str());
} else {
ROS_WARN_STREAM("Error switching controllers from " << stream_stop_controllers.str() << " to "
<< stream_start_controllers.str());
ROS_WARN_STREAM("Error switching controllers from " << stream_start_controllers.str() << " to "
<< stream_stop_controllers.str());
return -1;
}
......
<?xml version="1.0" ?>
<sdf version="1.5">
<world name="default">
<!-- A global light source -->
<include>
<uri>model://sun</uri>
</include>
<!-- A ground plane -->
<include>
<uri>model://ground_plane</uri>
</include>
<physics name='default_physics' default='0' type='ode'>
<!-- the max step size is set to 0.001 by default. -->
<!-- When increasing it, the simulation quality may suffer. -->
<max_step_size>0.002</max_step_size>
<!-- this factor actually is computed by multiplying the other two -->
<real_time_factor>0.2</real_time_factor>
<real_time_update_rate>100</real_time_update_rate>
</physics>
</world>
</sdf>