Skip to content
Snippets Groups Projects
Commit 217206c9 authored by Johannes Mey's avatar Johannes Mey
Browse files

remove everything that is not related to the simulation itself

parent 5a39440b
No related branches found
No related tags found
No related merge requests found
Showing
with 0 additions and 1442 deletions
...@@ -60,23 +60,11 @@ include_directories(${catkin_INCLUDE_DIRS} ${SDF_INCLUDE_DIRS}) ...@@ -60,23 +60,11 @@ include_directories(${catkin_INCLUDE_DIRS} ${SDF_INCLUDE_DIRS})
# add custom controller as library # add custom controller as library
add_library(${PROJECT_NAME}_controllers_lib src/joint_position_controller.cpp) add_library(${PROJECT_NAME}_controllers_lib src/joint_position_controller.cpp)
add_library(GazeboZoneSpawner src/SafetyZones/GazeboZoneSpawner.cpp src/SafetyZones/GazeboZoneSpawner.h)
add_library(SafetyEnvironmentCreator src/SafetyZones/SafetyEnvironmentCreator.cpp src/SafetyZones/SafetyEnvironmentCreator.h)
# Specify libraries to link a library or executable target against # Specify libraries to link a library or executable target against
target_link_libraries(${PROJECT_NAME}_controllers_lib ${catkin_LIBRARIES}) target_link_libraries(${PROJECT_NAME}_controllers_lib ${catkin_LIBRARIES})
target_link_libraries(GazeboZoneSpawner ${catkin_LIBRARIES} ${SDF_LIBRARIES})
target_link_libraries(SafetyEnvironmentCreator ${catkin_LIBRARIES})
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(robot_state_initializer_node src/robot_state_initializer.cpp)
add_executable(SafetyAwarePlanner src/SafetyAwarePlanner.cpp)
target_link_libraries(robot_control_node ${catkin_LIBRARIES} boost_filesystem ${JSONCPP_LIBRARIES} ${SDF_LIBRARIES} GazeboZoneSpawner)
target_link_libraries(box_publisher_node ${catkin_LIBRARIES})
target_link_libraries(robot_state_initializer_node ${catkin_LIBRARIES}) target_link_libraries(robot_state_initializer_node ${catkin_LIBRARIES})
target_link_libraries(SafetyAwarePlanner ${catkin_LIBRARIES} SafetyEnvironmentCreator GazeboZoneSpawner)
# target_link_libraries(constraint_planner ${catkin_LIBRARIES} ${Boost_LIBRARIES} SafetyEnvironmentCreator)
# panda_simulation # panda_simulation
![Panda in Gazebo](assets/panda-in-gazebo.png?raw=true "Panda in Gazebo") ![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"
```
==== BASE ====
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:
#### Safetyzones
![Zone](assets/box.png?raw=true "Safetyzone")
Integrates safetyzones via the SafetyAwarePlanner.
start-command:
...
roslaunch panda_simulation simulation.launch
...
To see through the boxes one have to set the alpha of the planning an environment (see picture).
#### _MoveIt!_ constraint-aware planning
An example of one can add constraints to the planning and simulate it. start-command:
start-command:
...
roslaunch panda_simulation simulation_constraint.launch
...
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")
assets/box.png

202 KiB

assets/robot-state-initializer.gif

117 KiB

<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>
...@@ -44,9 +44,6 @@ ...@@ -44,9 +44,6 @@
<include file="$(find panda_moveit_config)/launch/move_group.launch"> <include file="$(find panda_moveit_config)/launch/move_group.launch">
<arg name="load_gripper" value="$(arg load_gripper)" /> <arg name="load_gripper" value="$(arg load_gripper)" />
</include> </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" /> <node name="joint_state_desired_publisher" pkg="topic_tools" type="relay" args="joint_states joint_states_desired" />
...@@ -59,7 +56,4 @@ ...@@ -59,7 +56,4 @@
<!-- run custom node for automatic intialization --> <!-- 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="robot_state_initializer_node" name="robot_state_initializer_node" />
<node name="constraint_planner" pkg="panda_simulation" type="constraint_planner" respawn="false" output="screen"/>
</launch> </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" />
<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 name="SampleConstraintPlanner" pkg="panda_simulation" type="SampleConstraintPlanner" respawn="false" output="screen"/>
</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" />
<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 name="SampleConstraintPlanner" pkg="panda_simulation" type="SafetyAwarePlanner" respawn="false" output="screen"/>
</launch>
<?xml version="1.0" ?>
<sdf version="1.5">
<model name="box">
<static>true</static>
<link name="link">
<visual name="visual">
<material>
<script>
<uri>/usr/share/gazebo-9/media/materials/scripts/gazebo.material</uri>
<name>Gazebo/DarkMagentaTransparent</name>
</script>
</material>
<pose>2 3 0.5 0 0 0</pose>
<geometry>
<box>
<size>1 1 1</size>
</box>
</geometry>
</visual>
</link>
</model>
</sdf>
\ No newline at end of file
<?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
...@@ -31,7 +31,6 @@ ...@@ -31,7 +31,6 @@
<depend>std_msgs</depend> <depend>std_msgs</depend>
<depend>tf</depend> <depend>tf</depend>
<depend>xacro</depend> <depend>xacro</depend>
<depend>libjsoncpp-dev</depend>
<depend>moveit_simple_controller_manager</depend> <depend>moveit_simple_controller_manager</depend>
<!-- CUSTOM --> <!-- CUSTOM -->
...@@ -42,10 +41,6 @@ ...@@ -42,10 +41,6 @@
<depend>controller_interface</depend> <depend>controller_interface</depend>
<depend>hardware_interface</depend> <depend>hardware_interface</depend>
<build_depend>sdformat</build_depend>
<exec_depend>sdformat</exec_depend>
<exec_depend>gazebo_ros</exec_depend> <exec_depend>gazebo_ros</exec_depend>
<exec_depend>pluginlib</exec_depend> <exec_depend>pluginlib</exec_depend>
......
#include <moveit/move_group_interface/move_group_interface.h>
#include <moveit/planning_scene_interface/planning_scene_interface.h>
#include <moveit_msgs/CollisionObject.h>
#include <moveit_visual_tools/moveit_visual_tools.h>
#include "SafetyZones/SafetyEnvironmentCreator.cpp"
#include "SafetyZones/GazeboZoneSpawner.h"
int main(int argc, char** argv)
{
ros::init(argc, argv, "CONSTRAINT PLANNER");
ros::NodeHandle node_handle;
ros::AsyncSpinner spinner(1);
spinner.start();
static const std::string PLANNING_GROUP = "panda_arm";
moveit::planning_interface::MoveGroupInterface move_group(PLANNING_GROUP);
moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
const robot_state::JointModelGroup* joint_model_group =
move_group.getCurrentState()->getJointModelGroup(PLANNING_GROUP);
// Visualization Setup
namespace rvt = rviz_visual_tools;
moveit_visual_tools::MoveItVisualTools visual_tools("panda_link0");
//visual_tools.deleteAllMarkers();
visual_tools.loadRemoteControl();
visual_tools.setAlpha(0.5);
Eigen::Isometry3d text_pose = Eigen::Isometry3d::Identity();
text_pose.translation().z() = 1.75;
visual_tools.publishText(text_pose, "Sample constraint planner node", 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
ROS_INFO_NAMED("constraint_planner", "Planning frame: %s", move_group.getPlanningFrame().c_str());
ROS_INFO_NAMED("constraint_planner", "End effector link: %s", move_group.getEndEffectorLink().c_str());
ROS_INFO_NAMED("constraint_planner", "Available Planning Groups:");
std::copy(move_group.getJointModelGroupNames().begin(), move_group.getJointModelGroupNames().end(),
std::ostream_iterator<std::string>(std::cout, ", "));
shape_msgs::SolidPrimitive safety_box;
geometry_msgs::Pose safety_box_pose;
safety_box_pose.orientation.w = 1.0;
safety_box_pose.orientation.x = 0.0;
safety_box_pose.orientation.y = 0.0;
safety_box_pose.orientation.z = 0.0;
safety_box_pose.position.x = 0.0;
safety_box_pose.position.y = 0.0;
safety_box_pose.position.z = 0.0;
safety_box.type = safety_box.BOX;
safety_box.dimensions.resize(3);
safety_box.dimensions[0] = 2.0;
safety_box.dimensions[1] = 2.0;
safety_box.dimensions[2] = 2.0;
// Safety Box
SafetyEnvironmentCreator sec(safety_box, safety_box_pose);
std::vector<moveit_msgs::CollisionObject> collision_objects = sec.createCentralizedSafetyEnvironment(move_group);
// show safety box in gazebo
safety_box_pose.position.z += .5 * safety_box.BOX_Z;
GazeboZoneSpawner::spawnCollisionBox(safety_box, safety_box_pose);
//visual_tools.processCollisionObjectMsg(collision_objects[2], rviz_visual_tools::RED);
// visual_tools.setAlpha(0.0);
// 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();
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);
//moveit_msgs::MotionPlanRequest mr;
//mr.max_velocity_scaling_factor = 0.5;
//move_group.constructMotionPlanRequest(mr);
//move_group.setMaxVelocityScalingFactor(0.1);
//move_group.setWorkspace(-0.5, -0.5, -0.5, 0, 0, 0);
moveit::planning_interface::MoveGroupInterface::Plan my_plan;
bool success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
ROS_INFO_NAMED("constraint_planner", "Visualizing constraint plan %s", success ? "" : "FAILED");
visual_tools.prompt("Press 'next' to move the simulated robot.");
visual_tools.trigger();
// Move the simulated robot in gazebo
move_group.move();
ros::shutdown();
return 0;
}
\ No newline at end of file
//
// Created by Johannes Mey on 31.03.20.
//
#include "GazeboZoneSpawner.h"
#include <sdformat-6.0/sdf/sdf.hh> // for sdf model parsing
#include <gazebo_msgs/SpawnModel.h> // for spawning stuff in gazebo
#include <ros/ros.h>
#include <ros/package.h>
#include <tf/tf.h>
void GazeboZoneSpawner::spawnCollisionBox(shape_msgs::SolidPrimitive shape, geometry_msgs::Pose pose) {
if (shape.type != shape.BOX) {
ROS_ERROR("Safety-zone could not be created due to wrong shape-type");
return;
}
sdf::SDFPtr sdf(new sdf::SDF());
sdf::init(sdf);
auto sdfFileName = ros::package::getPath(ROS_PACKAGE_NAME) + "/models/box.sdf";
assert(sdf::readFile(sdfFileName, sdf));
auto visualElement = sdf->Root()->GetElement("model")->GetElement("link")->GetElement("visual");
auto poseElement = visualElement->GetElement("pose");
auto sizeElement = visualElement->GetElement("geometry")->GetElement("box")->GetElement("size");
tf::Quaternion rot;
rot.setValue(pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w);
double roll, pitch, yaw;
tf::Matrix3x3(rot).getRPY(roll, pitch, yaw);
std::ostringstream poseStream;
poseStream << pose.position.x << " " << pose.position.y << " " << pose.position.z << " " << roll << " " << pitch << " " << yaw;
poseElement->Set(poseStream.str());
ROS_INFO_NAMED("GazeboZoneSpawner", "Set pose in SDF file to '%s'.", poseElement->GetValue()->GetAsString().c_str());
std::ostringstream sizeStream;
sizeStream << shape.dimensions[0] << " " << shape.dimensions[1] << " " << shape.dimensions[2];
sizeElement->Set(sizeStream.str());
ROS_INFO_NAMED("GazeboZoneSpawner", "Set size in SDF file to '%s'.", sizeElement->GetValue()->GetAsString().c_str());
ros::NodeHandle n;
ros::ServiceClient client = n.serviceClient<gazebo_msgs::SpawnModel>("/gazebo/spawn_sdf_model");
gazebo_msgs::SpawnModel spawnService;
spawnService.request.robot_namespace = "box space";
// spawnModelService.request.initial_pose = pose; // not required here
spawnService.request.model_name = std::string("box") + poseStream.str() + sizeStream.str();
spawnService.request.model_xml = sdf->ToString().c_str();
// spawnModelService.request.reference_frame = ; // if left empty, world is used
if (client.call(spawnService)) {
if ( spawnService.response.success) {
ROS_INFO_NAMED("GazeboZoneSpawner", "Successfully spawned Box 's'. %s", spawnService.request.model_name.c_str(), spawnService.response.status_message.c_str());
} else {
ROS_INFO_NAMED("GazeboZoneSpawner", "Failed to spawn Box '%s'. %s", spawnService.request.model_name.c_str(), spawnService.response.status_message.c_str());
}
} else {
ROS_ERROR_NAMED("GazeboZoneSpawner", "Failed to call service '/gazebo/spawn_sdf_model'");
}
}
//
// Created by Johannes Mey on 31.03.20.
//
#ifndef PANDA_SIMULATION_GAZEBOZONESPAWNER_H
#define PANDA_SIMULATION_GAZEBOZONESPAWNER_H
#include <shape_msgs/SolidPrimitive.h>
#include <geometry_msgs/Pose.h>
class GazeboZoneSpawner {
public:
static void spawnCollisionBox(shape_msgs::SolidPrimitive shape, geometry_msgs::Pose pose);
};
#endif //PANDA_SIMULATION_GAZEBOZONESPAWNER_H
//
// Created by Sebastian Ebert
//
#include "SafetyEnvironmentCreator.h"
#include <moveit/move_group_interface/move_group_interface.h>
#include <moveit_msgs/CollisionObject.h>
SafetyEnvironmentCreator::SafetyEnvironmentCreator(shape_msgs::SolidPrimitive shape, geometry_msgs::Pose pose) {
if(shape.type != shape.BOX){
ROS_ERROR("Safety-zone could not be created due to wrong shape-type");
safety_zone_depth = 0.0;
safety_zone_width = 0.0;
safety_zone_height = 0.0;
zone_orientation_w = 1.0;
zone_orientation_x = 1.0;
zone_orientation_y = 1.0;
zone_orientation_z = 1.0;
}else {
safety_zone_depth = shape.dimensions[0];
safety_zone_width = shape.dimensions[1];
safety_zone_height = shape.dimensions[2];
zone_orientation_w = pose.orientation.w;
zone_orientation_x = pose.orientation.x;
zone_orientation_y = pose.orientation.y;
zone_orientation_z = pose.orientation.z;
zone_position_x = pose.position.x;
zone_position_y = pose.position.y;
zone_position_z = pose.position.z;
}
}
double SafetyEnvironmentCreator::getZoneWidth() const {
return safety_zone_width;
}
double SafetyEnvironmentCreator::getZoneHeight() const {
return safety_zone_height;
}
double SafetyEnvironmentCreator::getZoneDepth() const {
return safety_zone_depth;
}
double SafetyEnvironmentCreator::getZoneOrientationW() const {
return zone_orientation_w;
}
double SafetyEnvironmentCreator::getZoneOrientationX() const {
return zone_orientation_x;
}
double SafetyEnvironmentCreator::getZoneOrientationY() const {
return zone_orientation_y;
}
double SafetyEnvironmentCreator::getZoneOrientationZ() const {
return zone_orientation_z;
}
std::vector<moveit_msgs::CollisionObject>
SafetyEnvironmentCreator::createCentralizedSafetyEnvironment(moveit::planning_interface::MoveGroupInterface &move_group) {
std::vector<moveit_msgs::CollisionObject> collision_objects;
// OBJECT 1 ( left box / -y )
// ^^^^^^^^
// 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] = getZoneDepth() + 0.2; // x
primitive.dimensions[1] = 0.1; // y
primitive.dimensions[2] = getZoneHeight(); // z
// 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 = zone_position_x;
box_pose.position.y = zone_position_y + (-1 * ((getZoneWidth() / 2) + 0.05));
box_pose.position.z = zone_position_z + (primitive.dimensions[2] / 2);
collision_object.primitives.push_back(primitive);
collision_object.primitive_poses.push_back(box_pose);
collision_object.operation = collision_object.ADD;
collision_objects.push_back(collision_object);
// OBJECT 2 right box / +y )
// ^^^^^^^^
moveit_msgs::CollisionObject collision_object_2;
collision_object_2.header.frame_id = move_group.getPlanningFrame();
// The id of the object is used to identify it.
collision_object_2.id = "box2";
// Define a box to add to the world.
shape_msgs::SolidPrimitive primitive_2;
primitive_2.type = primitive.BOX;
primitive_2.dimensions.resize(3);
primitive_2.dimensions[0] = getZoneDepth() + 0.2;
primitive_2.dimensions[1] = 0.1;
primitive_2.dimensions[2] = getZoneHeight();
// Define a pose for the box (specified relative to frame_id)
geometry_msgs::Pose box_pose_2;
box_pose_2.orientation.w = 1.0;
box_pose_2.position.x = zone_position_x;
box_pose_2.position.y = zone_position_y + ((getZoneWidth() / 2) + 0.05);
box_pose_2.position.z = zone_position_z + (primitive_2.dimensions[2] / 2);
collision_object_2.primitives.push_back(primitive_2);
collision_object_2.primitive_poses.push_back(box_pose_2);
collision_object_2.operation = collision_object_2.ADD;
collision_objects.push_back(collision_object_2);
// OBJECT 3 (front box / +x)
// ^^^^^^^^
moveit_msgs::CollisionObject collision_object_3;
collision_object_3.header.frame_id = move_group.getPlanningFrame();
// The id of the object is used to identify it.
collision_object_3.id = "box3";
// Define a box to add to the world.
shape_msgs::SolidPrimitive primitive_3;
primitive_3.type = primitive.BOX;
primitive_3.dimensions.resize(3);
primitive_3.dimensions[0] = 0.1;
primitive_3.dimensions[1] = getZoneWidth() + 0.2;
primitive_3.dimensions[2] = getZoneHeight();
// Define a pose for the box (specified relative to frame_id)
geometry_msgs::Pose box_pose_3;
box_pose_3.orientation.w = 1.0;
box_pose_3.position.x = zone_position_x + ((getZoneDepth() / 2) + 0.05);
box_pose_3.position.y = zone_position_y;
box_pose_3.position.z = zone_position_z + (primitive_3.dimensions[2] / 2);
collision_object_3.primitives.push_back(primitive_3);
collision_object_3.primitive_poses.push_back(box_pose_3);
collision_object_3.operation = collision_object_3.ADD;
collision_objects.push_back(collision_object_3);
// OBJECT 4 (back box / -x)
// ^^^^^^^^
moveit_msgs::CollisionObject collision_object_4;
collision_object_4.header.frame_id = move_group.getPlanningFrame();
// The id of the object is used to identify it.
collision_object_4.id = "box4";
// Define a box to add to the world.
shape_msgs::SolidPrimitive primitive_4;
primitive_4.type = primitive.BOX;
primitive_4.dimensions.resize(3);
primitive_4.dimensions[0] = 0.1;
primitive_4.dimensions[1] = getZoneWidth() + 0.2;
primitive_4.dimensions[2] = getZoneHeight();
// Define a pose for the box (specified relative to frame_id)
geometry_msgs::Pose box_pose_4;
box_pose_4.orientation.w = 1.0;
box_pose_4.position.x = zone_position_x + (-1 * ((getZoneDepth() / 2) + 0.05));
box_pose_4.position.y = zone_position_y;
box_pose_4.position.z = zone_position_z + (primitive_4.dimensions[2] / 2);
collision_object_4.primitives.push_back(primitive_4);
collision_object_4.primitive_poses.push_back(box_pose_4);
collision_object_4.operation = collision_object_4.ADD;
collision_objects.push_back(collision_object_4);
// OBJECT 5 (floor / -z)
// ^^^^^^^^
moveit_msgs::CollisionObject collision_object_5;
collision_object_5.header.frame_id = move_group.getPlanningFrame();
// The id of the object is used to identify it.
collision_object_5.id = "box5";
// Define a box to add to the world.
shape_msgs::SolidPrimitive primitive_5;
primitive_5.type = primitive.BOX;
primitive_5.dimensions.resize(3);
primitive_5.dimensions[0] = getZoneDepth() + 0.2;
primitive_5.dimensions[1] = getZoneWidth() + 0.2;
primitive_5.dimensions[2] = 0.1;
// Define a pose for the box (specified relative to frame_id)
geometry_msgs::Pose box_pose_5;
box_pose_5.orientation.w = 1.0;
box_pose_5.position.x = zone_position_x;
box_pose_5.position.y = zone_position_y;
box_pose_5.position.z = zone_position_z - 0.05;
collision_object_5.primitives.push_back(primitive_5);
collision_object_5.primitive_poses.push_back(box_pose_5);
collision_object_5.operation = collision_object_5.ADD;
collision_objects.push_back(collision_object_5);
// OBJECT 6 (top / +z)
// ^^^^^^^^
moveit_msgs::CollisionObject collision_object_6;
collision_object_6.header.frame_id = move_group.getPlanningFrame();
// The id of the object is used to identify it.
collision_object_6.id = "box6";
// Define a box to add to the world.
shape_msgs::SolidPrimitive primitive_6;
primitive_6.type = primitive.BOX;
primitive_6.dimensions.resize(3);
primitive_6.dimensions[0] = getZoneDepth() + 0.2;
primitive_6.dimensions[1] = getZoneWidth() + 0.2;
primitive_6.dimensions[2] = 0.1;
// Define a pose for the box (specified relative to frame_id)
geometry_msgs::Pose box_pose_6;
box_pose_6.orientation.w = 1.0;
box_pose_6.position.x = zone_position_x;
box_pose_6.position.y = zone_position_y;
box_pose_6.position.z = zone_position_z + getZoneHeight() + 0.05;
collision_object_6.primitives.push_back(primitive_6);
collision_object_6.primitive_poses.push_back(box_pose_6);
collision_object_6.operation = collision_object_6.ADD;
collision_objects.push_back(collision_object_6);
/*for(std::size_t i=0; i<collision_objects.size(); ++i) {
collision_objects[i].primitive_poses[0].orientation.w = getZoneOrientationW();
collision_objects[i].primitive_poses[0].orientation.x = getZoneOrientationX();
collision_objects[i].primitive_poses[0].orientation.y = getZoneOrientationY();
collision_objects[i].primitive_poses[0].orientation.z = getZoneOrientationZ();
}*/
return collision_objects;
}
//
// Created by Johannes Mey on 31.03.20.
//
#ifndef PANDA_SIMULATION_SAFETYENVIRONMENTCREATOR_H
#define PANDA_SIMULATION_SAFETYENVIRONMENTCREATOR_H
#include <moveit/move_group_interface/move_group_interface.h>
#include <moveit_msgs/CollisionObject.h>
#include <moveit_visual_tools/moveit_visual_tools.h>
class SafetyEnvironmentCreator {
private:
double safety_zone_width;
double safety_zone_height;
double safety_zone_depth;
double zone_orientation_w;
double zone_orientation_x;
double zone_orientation_y;
double zone_orientation_z;
double zone_position_x;
double zone_position_y;
double zone_position_z;
public:
double getZoneWidth() const;
double getZoneHeight() const;
double getZoneDepth() const;
double getZoneOrientationW() const;
double getZoneOrientationX() const;
double getZoneOrientationY() const;
double getZoneOrientationZ() const;
/**
* ctor
* @param shape describes the inner dimensions of the safety zone
* @param pose describes position and orientation of the safety zone
* note: current zone-threshold is 0.1
*/
SafetyEnvironmentCreator(shape_msgs::SolidPrimitive shape, geometry_msgs::Pose pose);
std::vector<moveit_msgs::CollisionObject>
createCentralizedSafetyEnvironment(moveit::planning_interface::MoveGroupInterface &move_group);
};
#endif //PANDA_SIMULATION_SAFETYENVIRONMENTCREATOR_H
//
// Created by sebastian on 27.03.20.
//
#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, "CONSTRAINT PLANNER");
ros::NodeHandle node_handle;
ros::AsyncSpinner spinner(1);
spinner.start();
static const std::string PLANNING_GROUP = "panda_arm";
moveit::planning_interface::MoveGroupInterface move_group(PLANNING_GROUP);
moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
const robot_state::JointModelGroup* joint_model_group =
move_group.getCurrentState()->getJointModelGroup(PLANNING_GROUP);
// Visualization Setup
namespace rvt = rviz_visual_tools;
moveit_visual_tools::MoveItVisualTools visual_tools("panda_link0");
visual_tools.deleteAllMarkers();
visual_tools.loadRemoteControl();
Eigen::Isometry3d text_pose = Eigen::Isometry3d::Identity();
text_pose.translation().z() = 1.75;
visual_tools.publishText(text_pose, "Sample constraint planner node", 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
ROS_INFO_NAMED("constraint_planner", "Planning frame: %s", move_group.getPlanningFrame().c_str());
ROS_INFO_NAMED("constraint_planner", "End effector link: %s", move_group.getEndEffectorLink().c_str());
ROS_INFO_NAMED("constraint_planner", "Available Planning Groups:");
std::copy(move_group.getJointModelGroupNames().begin(), move_group.getJointModelGroupNames().end(),
std::ostream_iterator<std::string>(std::cout, ", "));
// 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("constraint_planner", "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();
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);
moveit::planning_interface::MoveGroupInterface::Plan my_plan;
bool success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
ROS_INFO_NAMED("constraint_planner", "Visualizing constraint plan %s", success ? "" : "FAILED");
visual_tools.prompt("Press 'next' to move the simulated robot.");
visual_tools.trigger();
// Move the simulated robot in gazebo
move_group.move();
ros::shutdown();
return 0;
}
\ 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;
}
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment