diff --git a/CMakeLists.txt b/CMakeLists.txt index 5d364d891ba61031680e95c66693dc60bb2b63eb..483538647a3d35dd83808e34e3ab3dfa20ee6a36 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -67,11 +67,11 @@ target_link_libraries(${PROJECT_NAME}_controllers_lib ${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(constraint_planner src/constraint_planner.cpp src/SafetyEnvironmentCreator.cpp) +add_executable(SafetyAwarePlanner src/SafetyAwarePlanner.cpp src/SafetyEnvironmentCreator.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(constraint_planner ${catkin_LIBRARIES}) +target_link_libraries(SafetyAwarePlanner ${catkin_LIBRARIES}) #target_link_libraries(constraint_planner ${catkin_LIBRARIES} ${Boost_LIBRARIES} SafetyEnvironmentCreator) diff --git a/README.md b/README.md index 1027e043c071b1a1ea4bae366bea4593aad9e5c0..0bfd3e1382d64e43c3adeb1127724219ace019e5 100644 --- a/README.md +++ b/README.md @@ -38,7 +38,16 @@ 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 +67,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_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/constraint_planner.cpp b/src/SafetyAwarePlanner.cpp similarity index 89% rename from src/constraint_planner.cpp rename to src/SafetyAwarePlanner.cpp index 966f9291675afc2f9193d54a530095b82f06ba22..b9fb3cf85cf9ec3b64071342b84e9436ff2605cc 100644 --- a/src/constraint_planner.cpp +++ b/src/SafetyAwarePlanner.cpp @@ -44,8 +44,17 @@ int main(int argc, char** argv) std::copy(move_group.getJointModelGroupNames().begin(), move_group.getJointModelGroupNames().end(), std::ostream_iterator<std::string>(std::cout, ", ")); + shape_msgs::SolidPrimitive safetybox; + geometry_msgs::Pose box_pose; + safetybox.type = safetybox.BOX; + safetybox.dimensions.resize(3); + + safetybox.dimensions[0] = 2.0; + safetybox.dimensions[1] = 2.0; + safetybox.dimensions[2] = 2.0; + // Safety Box - SafetyEnvironmentCreator sec(2.0, 2.0 , 2.0); + SafetyEnvironmentCreator sec(safetybox, box_pose); std::vector<moveit_msgs::CollisionObject> collision_objects = sec.createCentralizedSafetyEnvironment(move_group); //visual_tools.processCollisionObjectMsg(collision_objects[2], rviz_visual_tools::RED); @@ -69,12 +78,13 @@ int main(int argc, char** argv) 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); + + //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"); @@ -84,6 +94,7 @@ int main(int argc, char** argv) // Move the simulated robot in gazebo move_group.move(); + move_group.stop(); ros::shutdown(); return 0; diff --git a/src/SafetyEnvironmentCreator.cpp b/src/SafetyEnvironmentCreator.cpp index 171342a66621b0bf3fb3e4a1b44089bda00f18a8..4e36529a174bb4845ab08f13ddb8ab7e3d25f1ea 100644 --- a/src/SafetyEnvironmentCreator.cpp +++ b/src/SafetyEnvironmentCreator.cpp @@ -14,11 +14,23 @@ class SafetyEnvironmentCreator { double safety_zone_depth; public: - // ctor - SafetyEnvironmentCreator(double zone_width, double zone_height, double zone_depth) { - safety_zone_width = zone_width; - safety_zone_height = zone_height; - safety_zone_depth = zone_depth; + /** + * 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; + }else { + safety_zone_depth = shape.dimensions[0]; + safety_zone_width = shape.dimensions[1]; + safety_zone_height = shape.dimensions[2]; + } } double getZoneWidth() { 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