diff --git a/CMakeLists.txt b/CMakeLists.txt
index 32e74b5ad40d95ab55b9372ac449564119885d3e..7667d14ede32d96319d897eb6e302784a022b414 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -58,18 +58,22 @@ catkin_package(CATKIN_DEPENDS
# Specify additional locations of header files Your package locations should be listed before other locations
include_directories(${catkin_INCLUDE_DIRS} ${SDF_INCLUDE_DIRS})
+# add custom controller as library
+add_library(${PROJECT_NAME}_controllers_lib src/joint_position_controller.cpp)
+#add_library(SafetyEnvironmentCreator src/SafetyEnvironmentCreator.cpp)
+
+# Specify libraries to link a library or executable target against
+target_link_libraries(${PROJECT_NAME}_controllers_lib ${catkin_LIBRARIES})
+#target_link_libraries(SafetyEnvironmentCreator ${catkin_LIBRARIES} ${Boost_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(move_group_interface_tutorial src/move_group_interface_tutorial.cpp)
+add_executable(SafetyAwarePlanner src/SafetyAwarePlanner.cpp src/SafetyEnvironmentCreator.cpp)
target_link_libraries(robot_control_node ${catkin_LIBRARIES} boost_filesystem ${JSONCPP_LIBRARIES} ${SDF_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})
+target_link_libraries(SafetyAwarePlanner ${catkin_LIBRARIES})
+#target_link_libraries(constraint_planner ${catkin_LIBRARIES} ${Boost_LIBRARIES} SafetyEnvironmentCreator)
-# add custom controller as library
-add_library(${PROJECT_NAME}_controllers_lib src/joint_position_controller.cpp)
-
-# Specify libraries to link a library or executable target against
-target_link_libraries(${PROJECT_NAME}_controllers_lib ${catkin_LIBRARIES})
diff --git a/README.md b/README.md
index 1027e043c071b1a1ea4bae366bea4593aad9e5c0..23cff5c0b1e38c06771b987fab0618d588dc5994 100644
--- a/README.md
+++ b/README.md
@@ -38,7 +38,25 @@ 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)
+ #### Safetyzones
+
+ 
+
+ 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.
@@ -58,4 +76,4 @@ You can see the full explanation in my [blog post](https://erdalpekel.de/?p=55).
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.
-
\ No newline at end of file
+
diff --git a/assets/box.png b/assets/box.png
new file mode 100644
index 0000000000000000000000000000000000000000..391c0c2e3b35358d689f769308d4b6059466aae4
Binary files /dev/null and b/assets/box.png differ
diff --git a/launch/simulation.launch b/launch/simulation.launch
index f2583abfa5cf6c0d2b59d151b9cae3dd44a05a39..a982c720e046458a10b05d96603481485084859e 100644
--- a/launch/simulation.launch
+++ b/launch/simulation.launch
@@ -27,7 +27,6 @@
<!-- 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" />
@@ -58,4 +57,14 @@
<!-- run custom node for automatic intialization -->
<node pkg="panda_simulation" type="robot_state_initializer_node" name="robot_state_initializer_node" />
+ <!--<node name="move_group_interface_tutorial" pkg="panda_simulation" type="move_group_interface_tutorial" respawn="false" output="screen"/>-->
+
+ <node name="constraint_planner" pkg="panda_simulation" type="constraint_planner" respawn="false" output="screen"/>
+
+ <!--<node name="motion_planning_api_tutorial" pkg="panda_simulation" type="motion_planning_api_tutorial" respawn="false" output="screen">
+ <rosparam command="load" file="$(find panda_moveit_config)/config/kinematics.yaml"/>
+ <param name="/planning_plugin" value="ompl_interface/OMPLPlanner"/>
+ <rosparam command="load" file="$(find panda_moveit_config)/config/ompl_planning.yaml"/>
+ </node>-->
+
</launch>
diff --git a/launch/simulation_constraint.launch b/launch/simulation_constraint.launch
new file mode 100644
index 0000000000000000000000000000000000000000..104280310e0a4d95470caa745bec8dc0c574f966
--- /dev/null
+++ b/launch/simulation_constraint.launch
@@ -0,0 +1,62 @@
+<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>
diff --git a/src/SafetyAwarePlanner.cpp b/src/SafetyAwarePlanner.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..460e9ae5efdee20eb5d37305c246c8a659916a8e
--- /dev/null
+++ b/src/SafetyAwarePlanner.cpp
@@ -0,0 +1,104 @@
+#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 "SafetyEnvironmentCreator.cpp"
+
+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);
+
+ //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
diff --git a/src/SafetyEnvironmentCreator.cpp b/src/SafetyEnvironmentCreator.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..f6ccedb8084b4e384add979d5a65d05fd07e0b7d
--- /dev/null
+++ b/src/SafetyEnvironmentCreator.cpp
@@ -0,0 +1,270 @@
+//
+// Created by Sebastian Ebert
+//
+
+#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:
+ /**
+ * 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) {
+ 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 getZoneWidth() const {
+ return safety_zone_width;
+ }
+
+ double getZoneHeight() const {
+ return safety_zone_height;
+ }
+
+ double getZoneDepth() const {
+ return safety_zone_depth;
+ }
+
+ double getZoneOrientationW() const {
+ return zone_orientation_w;
+ }
+
+ double getZoneOrientationX() const {
+ return zone_orientation_x;
+ }
+
+ double getZoneOrientationY() const {
+ return zone_orientation_y;
+ }
+
+ double getZoneOrientationZ() const {
+ return zone_orientation_z;
+ }
+
+ std::vector<moveit_msgs::CollisionObject> 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;
+ }
+};
\ No newline at end of file
diff --git a/src/SampleConstraintPlanner.cpp b/src/SampleConstraintPlanner.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..59e35a4988a7540a2134a4d2454cb1c8b0a3b23b
--- /dev/null
+++ b/src/SampleConstraintPlanner.cpp
@@ -0,0 +1,107 @@
+//
+// 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