Skip to content
Snippets Groups Projects
Commit 64243a07 authored by Sebastian Ebert's avatar Sebastian Ebert
Browse files

Merge branch 'summer_school_merge_branche' into 'master' 

updated applications for summer school

See merge request ceti/ros-internal/sample_applications!1
parents 7ef30876 4f7f2ba2
No related branches found
No related tags found
No related merge requests found
Show whitespace changes
Inline Side-by-side
Showing
with 589 additions and 147 deletions
CMakeLists.txt
+ 10
− 10
View file @ 64243a07
...@@ -51,13 +51,13 @@ catkin_package(CATKIN_DEPENDS ...@@ -51,13 +51,13 @@ catkin_package(CATKIN_DEPENDS
# Specify additional locations of header files Your package locations should be listed before other locations # Specify additional locations of header files Your package locations should be listed before other locations
include_directories(${catkin_INCLUDE_DIRS}) include_directories(${catkin_INCLUDE_DIRS})
add_executable(SampleConstraintPlanner src/SampleConstraintPlanner.cpp) add_executable(BasicCartesianPlanner src/BasicCartesianPlanner.cpp)
add_executable(SampleTimedCartesianPlanner src/SampleTimedCartesianPlanner.cpp) add_executable(BasicJointSpacePlanner src/BasicJointSpacePlanner.cpp)
add_executable(SampleSimpleMotion src/SampleSimpleMotion.cpp) add_executable(ObstacleAwarePlanner src/ObstacleAwarePlanner.cpp)
add_executable(MinimalSimpleMotion src/MinimalSimpleMotion.cpp) add_executable(GraspPlanner src/GraspPlanner.cpp)
# Specify libraries to link a library or executable target against ## Add cmake target dependencies of the executable
target_link_libraries(SampleConstraintPlanner ${catkin_LIBRARIES}) target_link_libraries(BasicCartesianPlanner ${catkin_LIBRARIES})
target_link_libraries(SampleTimedCartesianPlanner ${catkin_LIBRARIES}) target_link_libraries(BasicJointSpacePlanner ${catkin_LIBRARIES})
target_link_libraries(SampleSimpleMotion ${catkin_LIBRARIES}) target_link_libraries(ObstacleAwarePlanner ${catkin_LIBRARIES})
target_link_libraries(MinimalSimpleMotion ${catkin_LIBRARIES}) target_link_libraries(GraspPlanner ${catkin_LIBRARIES})
launch/simple_simulation.launch → launch/basic_cartesian_planner.launch
+ 1
− 3
View file @ 64243a07
...@@ -4,8 +4,6 @@ ...@@ -4,8 +4,6 @@
<include file="$(find panda_moveit_config)/launch/moveit_rviz.launch" /> <include file="$(find panda_moveit_config)/launch/moveit_rviz.launch" />
<node name="rqt_console" pkg="rqt_console" type="rqt_console" /> <node pkg="sample_applications" type="BasicCartesianPlanner" name="BasicCartesianPlannerInstance" respawn="false" output="screen"/>
<node pkg="sample_applications" type="SampleSimpleMotion" name="SampleSimpleMotion" />
</launch> </launch>
launch/constraint_simulation.launch → launch/basic_joint_space_planner.launch
+ 1
− 3
View file @ 64243a07
...@@ -4,8 +4,6 @@ ...@@ -4,8 +4,6 @@
<include file="$(find panda_moveit_config)/launch/moveit_rviz.launch" /> <include file="$(find panda_moveit_config)/launch/moveit_rviz.launch" />
<node name="rqt_console" pkg="rqt_console" type="rqt_console" /> <node pkg="sample_applications" type="BasicJointSpacePlanner" name="BasicJointSpacePlannerInstance" respawn="false" output="screen"/>
<node pkg="sample_applications" type="SampleConstraintPlanner" name="SampleConstraintPlanner" />
</launch> </launch>
launch/grasp_planner.launch 0 → 100644
+ 13
− 0
View file @ 64243a07
<launch>
<include file="$(find panda_simulation)/launch/simulation.launch"/>
<include file="$(find panda_moveit_config)/launch/moveit_rviz.launch" />
<node name="spawn_object" pkg="gazebo_ros" type="spawn_model" args="-urdf -z 1 -file $(find tutorial_moveit_motion_planning)/models/object.urdf -model c_object" respawn="false" output="screen" />
<node name="spawn_table1" pkg="gazebo_ros" type="spawn_model" args="-urdf -z 1 -file $(find tutorial_moveit_motion_planning)/models/table1.urdf -model c_table1" respawn="false" output="screen" />
<node name="spawn_table2" pkg="gazebo_ros" type="spawn_model" args="-urdf -z 1 -file $(find tutorial_moveit_motion_planning)/models/table2.urdf -model c_table2" respawn="false" output="screen" />
<node pkg="sample_applications" type="GraspPlanner" name="GraspPlannerInstance" respawn="false" output="screen"/>
</launch>
launch/minimal_simulation.launch deleted 100644 → 0
+ 0
− 7
View file @ 7ef30876
<launch>
<include file="$(find panda_simulation)/launch/simulation.launch"/>
<node pkg="sample_applications" type="MinimalSimpleMotion" name="MinimalSimpleMotion" />
</launch>
launch/timed_cartesian_simulation.launch → launch/obstacle_aware_planner.launch
+ 11
− 0
View file @ 64243a07
...@@ -4,8 +4,8 @@ ...@@ -4,8 +4,8 @@
<include file="$(find panda_moveit_config)/launch/moveit_rviz.launch" /> <include file="$(find panda_moveit_config)/launch/moveit_rviz.launch" />
<node name="rqt_console" pkg="rqt_console" type="rqt_console" /> <node name="spawn_box" pkg="gazebo_ros" type="spawn_model" args="-urdf -z 1 -file $(find tutorial_moveit_motion_planning)/models/box.urdf -model box" respawn="false" output="screen" />
<node pkg="sample_applications" type="SampleTimedCartesianPlanner" name="SampleTimedCartesianPlanner" /> <node pkg="sample_applications" type="ObstacleAwarePlanner" name="ObstacleAwarePlannerInstance" respawn="false" output="screen"/>
</launch> </launch>
models/box.urdf 0 → 100644
+ 29
− 0
View file @ 64243a07
<robot name="box">
<link name="world"/>
<gazebo reference="world">
<static>true</static>
</gazebo>
<joint name="fixed" type="fixed">
<parent link="world"/>
<child link="real_box"/>
</joint>
<link name="real_box">
<inertial>
<origin xyz="0.45 -0.2 0.0" />
<mass value="1.0" />
<inertia ixx="0.0" ixy="0.0" ixz="0.0" iyy="0.0" iyz="0.0" izz="0.0" />
</inertial>
<visual>
<origin xyz="0.45 -0.2 0.0"/>
<geometry>
<box size="0.4 0.1 0.3" />
</geometry>
</visual>
<collision>
<origin xyz="0.45 -0.2 0.0"/>
<geometry>
<box size="0.4 0.1 0.3" />
</geometry>
</collision>
</link>
</robot>
\ No newline at end of file
src/SampleTimedCartesianPlanner.cpp → src/BasicCartesianPlanner.cpp
+ 100
− 0
View file @ 64243a07
#include <moveit/move_group_interface/move_group_interface.h> #include <moveit/move_group_interface/move_group_interface.h>
#include <moveit_msgs/DisplayRobotState.h>
#include <moveit_msgs/DisplayTrajectory.h> #include <moveit_msgs/DisplayTrajectory.h>
#include <moveit_msgs/AttachedCollisionObject.h>
#include <moveit_msgs/CollisionObject.h> #include <moveit_msgs/CollisionObject.h>
#include <moveit_visual_tools/moveit_visual_tools.h> #include <moveit_visual_tools/moveit_visual_tools.h>
#include <moveit/trajectory_processing/iterative_time_parameterization.h> #include <moveit/trajectory_processing/iterative_time_parameterization.h>
#include <trajectory_msgs/JointTrajectoryPoint.h> #include <trajectory_msgs/JointTrajectoryPoint.h>
/** /**
* allows time/velocity-constraint planning for cartesian paths * simple demo of a robotic motions based on cartesian paths
*/ */
int main(int argc, char **argv) int main(int argc, char **argv) {
{ // init the node
ros::init(argc, argv, "sample_timed_cartesian"); ros::init(argc, argv, "basic_cartesian_planner");
ros::NodeHandle node_handle; ros::NodeHandle node_handle;
ros::AsyncSpinner spinner(1); ros::AsyncSpinner spinner(1);
spinner.start(); spinner.start();
// wait for robot init of robot_state_initializer // setup the planning environment
ROS_INFO_NAMED("constraint_planner", ">>>>>>>>>>>>>>>>> WAITING FOR ROBOT INIT <<<<<<<<<<<<<<<<"); static const std::string PLANNING_GROUP = "panda_arm";
ros::Duration(5.0).sleep(); moveit::planning_interface::MoveGroupInterface group(PLANNING_GROUP);
ROS_INFO_NAMED("constraint_planner", ">>>>>>>>>>>>>>>>> WAKING UP AFTER INIT <<<<<<<<<<<<<<<<");
// Visualization Setup // visualization setup
// MoveIts visual tools provide capabilities for visualizing objects, robots, and trajectories in RViz
namespace rvt = rviz_visual_tools; namespace rvt = rviz_visual_tools;
moveit_visual_tools::MoveItVisualTools visual_tools("panda_link0"); 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 timed cartesian planner node", rvt::WHITE, rvt::XLARGE);
// Batch publishing is used to reduce the number of messages being sent to RViz for large visualizations // Batch publishing is used to reduce the number of messages being sent to RViz for large visualizations
visual_tools.trigger(); visual_tools.enableBatchPublishing();
moveit::planning_interface::MoveGroupInterface group("panda_arm"); // 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();
ros::Duration(3.0).sleep();
ros::Publisher display_publisher = node_handle.advertise<moveit_msgs::DisplayTrajectory>("/move_group/display_planned_path", 1, true); // we can use visual_tools to wait for user input
moveit_msgs::DisplayTrajectory display_trajectory; visual_tools.prompt("Press 'next' to move the simulated robot. \n");
visual_tools.trigger();
moveit::planning_interface::MoveGroupInterface::Plan plan;
group.setStartStateToCurrentState(); group.setStartStateToCurrentState();
// 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.
std::vector<geometry_msgs::Pose> waypoints; std::vector<geometry_msgs::Pose> waypoints;
// initial pose of the robot // initial pose of the robot
...@@ -71,46 +69,30 @@ int main(int argc, char **argv) ...@@ -71,46 +69,30 @@ int main(int argc, char **argv)
target_pose_3.position.x = -0.6; target_pose_3.position.x = -0.6;
waypoints.push_back(target_pose_3); waypoints.push_back(target_pose_3);
moveit_msgs::RobotTrajectory trajectory_msg;
group.setPlanningTime(10.0); group.setPlanningTime(10.0);
double fraction = group.computeCartesianPath(waypoints,0.01,0.0,trajectory_msg, false); // create a plan object and compute the cartesian motion
moveit::planning_interface::MoveGroupInterface::Plan plan;
// The trajectory needs to be modified so it will include velocities as well. moveit_msgs::RobotTrajectory trajectory_msg;
// Create a RobotTrajectory object group.computeCartesianPath(waypoints, 0.01, 0.0, trajectory_msg, true);
robot_trajectory::RobotTrajectory rt(group.getCurrentState()->getRobotModel(), "panda_arm"); plan.trajectory_ = trajectory_msg;
// Get a RobotTrajectory from trajectory
rt.setRobotTrajectoryMsg(*group.getCurrentState(), trajectory_msg);
// Create a IterativeParabolicTimeParameterization object // uncomment to modify maximum velocity and acceleration of the motion to be planned
/*
robot_trajectory::RobotTrajectory rt(group.getCurrentState()->getRobotModel(), PLANNING_GROUP);
trajectory_processing::IterativeParabolicTimeParameterization iptp; trajectory_processing::IterativeParabolicTimeParameterization iptp;
// Compute computeTimeStamps
bool success = iptp.computeTimeStamps(rt);
ROS_INFO("Computed time stamp %s",success?"SUCCEDED":"FAILED");
rt.getRobotTrajectoryMsg(trajectory_msg);
// adjust velocities, accelerations and time_from_start double max_velocity_scaling_factor = 0.1;
for(int i = 0; i < trajectory_msg.joint_trajectory.points.size(); i++) double max_acceleration_scaling_factor = 0.1;
{
trajectory_msg.joint_trajectory.points[i].time_from_start.operator*=(2);
for(int j = 0; j < trajectory_msg.joint_trajectory.points.at(i).velocities.size(); j++)
{
if(trajectory_msg.joint_trajectory.points.at(i).velocities.at(j) != 0.0){
trajectory_msg.joint_trajectory.points.at(i).velocities.at(j) /= 2;
}
}
for(int j = 0; j < trajectory_msg.joint_trajectory.points.at(i).accelerations.size(); j++) iptp.computeTimeStamps(rt, max_velocity_scaling_factor, max_acceleration_scaling_factor);
{ rt.getRobotTrajectoryMsg(trajectory_msg);
trajectory_msg.joint_trajectory.points.at(i).accelerations.at(j) = */
sqrt(std::abs(trajectory_msg.joint_trajectory.points.at(i).accelerations.at(j)));
}
}
plan.trajectory_ = trajectory_msg; visual_tools.trigger();
ROS_INFO("Visualizing plan 4 (cartesian path) (%.2f%% acheived)",fraction * 100.0); visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to move the simulated robot");
// execute the planned motion
group.execute(plan); group.execute(plan);
ros::shutdown(); ros::shutdown();
......
src/SampleSimpleMotion.cpp → src/BasicJointSpacePlanner.cpp
+ 35
− 30
View file @ 64243a07
#include <moveit/move_group_interface/move_group_interface.h> #include <moveit/move_group_interface/move_group_interface.h>
#include <moveit/planning_scene_interface/planning_scene_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_msgs/CollisionObject.h>
#include <moveit_visual_tools/moveit_visual_tools.h> #include <moveit_visual_tools/moveit_visual_tools.h>
...@@ -14,45 +10,45 @@ ...@@ -14,45 +10,45 @@
/** /**
* simple demo of a robotic motion * simple demo of a robotic motion
*/ */
int main(int argc, char** argv) int main(int argc, char **argv) {
{ // init the node
ros::init(argc, argv, "SIMPLE PLANNER"); ros::init(argc, argv, "basic_joint_space_planner");
ros::NodeHandle node_handle; ros::NodeHandle node_handle;
ros::AsyncSpinner spinner(1); ros::AsyncSpinner spinner(1);
spinner.start(); spinner.start();
// wait for robot init of robot_state_initializer // setup the planning environment
ROS_INFO_NAMED("simple_planner", ">>>>>>>>>>>>>>>>> WAITING FOR ROBOT INIT <<<<<<<<<<<<<<<<");
ros::Duration(5.0).sleep();
ROS_INFO_NAMED("simple_planner", ">>>>>>>>>>>>>>>>> WAKING UP AFTER INIT <<<<<<<<<<<<<<<<");
static const std::string PLANNING_GROUP = "panda_arm"; static const std::string PLANNING_GROUP = "panda_arm";
moveit::planning_interface::MoveGroupInterface move_group(PLANNING_GROUP); moveit::planning_interface::MoveGroupInterface move_group(PLANNING_GROUP);
moveit::planning_interface::PlanningSceneInterface planning_scene_interface; moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
const robot_state::JointModelGroup* joint_model_group =
move_group.getCurrentState()->getJointModelGroup(PLANNING_GROUP);
// Visualization Setup // visualization setup
// MoveIts visual tools provide capabilities for visualizing objects, robots, and trajectories in RViz
namespace rvt = rviz_visual_tools; namespace rvt = rviz_visual_tools;
moveit_visual_tools::MoveItVisualTools visual_tools("panda_link0"); 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, "simple_planner node", rvt::WHITE, rvt::XLARGE);
// Batch publishing is used to reduce the number of messages being sent to RViz for large visualizations // Batch publishing is used to reduce the number of messages being sent to RViz for large visualizations
visual_tools.enableBatchPublishing();
// 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();
ros::Duration(3.0).sleep();
// we can use visual_tools to wait for user input
visual_tools.prompt("Press 'next' to plan the motion. \n");
visual_tools.trigger(); visual_tools.trigger();
// Getting Basic Information // getting basic information
ROS_INFO_NAMED("simple_planner", "Planning frame: %s", move_group.getPlanningFrame().c_str()); ROS_INFO("Planning frame: %s", move_group.getPlanningFrame().c_str());
ROS_INFO_NAMED("simple_planner", "End effector link: %s", move_group.getEndEffectorLink().c_str()); ROS_INFO("End effector link: %s", move_group.getEndEffectorLink().c_str());
ROS_INFO_NAMED("simple_planner", "Available Planning Groups:"); ROS_INFO("Available Planning Groups:");
std::copy(move_group.getJointModelGroupNames().begin(), move_group.getJointModelGroupNames().end(), std::copy(move_group.getJointModelGroupNames().begin(), move_group.getJointModelGroupNames().end(),
std::ostream_iterator<std::string>(std::cout, ", ")); std::ostream_iterator<std::string>(std::cout, ", "));
move_group.setStartState(*move_group.getCurrentState()); move_group.setStartStateToCurrentState();
// define start and target pose (by specifying the desired pose of the end-effector)
geometry_msgs::Pose another_pose; geometry_msgs::Pose another_pose;
another_pose.orientation.w = 1.0; another_pose.orientation.w = 1.0;
another_pose.position.x = 0.4; another_pose.position.x = 0.4;
...@@ -60,14 +56,23 @@ int main(int argc, char** argv) ...@@ -60,14 +56,23 @@ int main(int argc, char** argv)
another_pose.position.z = 0.9; another_pose.position.z = 0.9;
move_group.setPoseTarget(another_pose); move_group.setPoseTarget(another_pose);
// uncomment to modify maximum velocity and acceleration of the motion to be planned
/*
move_group.setMaxVelocityScalingFactor(0.1);
move_group.setMaxAccelerationScalingFactor(0.1);
*/
// create a plan object and plan the motion
moveit::planning_interface::MoveGroupInterface::Plan my_plan; moveit::planning_interface::MoveGroupInterface::Plan my_plan;
bool success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS); bool success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
ROS_INFO_NAMED("simple_planner", "Visualizing constraint plan %s", success ? "" : "FAILED"); ROS_INFO("Visualizing plan %s", success ? "" : "FAILED");
visual_tools.prompt("Press 'next' to move the simulated robot.");
visual_tools.trigger(); visual_tools.trigger();
visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo");
// Move the simulated robot in gazebo // Move the (simulated) robot in gazebo
move_group.move(); move_group.move();
ros::shutdown(); ros::shutdown();
......
src/GraspPlanner.cpp 0 → 100644
+ 352
− 0
View file @ 64243a07
// ROS
#include <ros/ros.h>
// MoveIt!
#include <moveit/planning_scene_interface/planning_scene_interface.h>
#include <moveit/move_group_interface/move_group_interface.h>
// TF2
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
geometry_msgs::Quaternion q_pick_object;
geometry_msgs::Pose getTargetPickPose(){
std::vector<double> co_dim{0.02, 0.02, 0.2};
std::vector<double> co_pose{0.5, 0.0, 0.2};
geometry_msgs::Pose target_pose;
tf2::Quaternion orientation_gripper(-0.924, 0.383, 0, 0);
tf2::Quaternion orientation_object(q_pick_object.x, q_pick_object.y, q_pick_object.z, q_pick_object.w);
tf2::Quaternion orientation = orientation_gripper * orientation_object;
orientation.setZ(orientation.getZ() * -1);
orientation.setY(orientation.getY() * -1);
orientation.setX(orientation.getX() * -1);
target_pose.orientation = tf2::toMsg(orientation);
target_pose.position.x = co_pose[0];
target_pose.position.y = co_pose[1];
target_pose.position.z = co_pose[2] + 0.3;
return target_pose;
}
void openGripper(trajectory_msgs::JointTrajectory& posture)
{
// BEGIN_SUB_TUTORIAL open_gripper
/* Add both finger joints of panda robot. */
posture.joint_names.resize(2);
posture.joint_names[0] = "panda_finger_joint1";
posture.joint_names[1] = "panda_finger_joint2";
/* Set them as open, wide enough for the object to fit. */
posture.points.resize(1);
posture.points[0].positions.resize(2);
posture.points[0].positions[0] = 0.04;
posture.points[0].positions[1] = 0.04;
posture.points[0].time_from_start = ros::Duration(0.5);
// END_SUB_TUTORIAL
}
void closedGripper(trajectory_msgs::JointTrajectory& posture)
{
// BEGIN_SUB_TUTORIAL closed_gripper
/* Add both finger joints of panda robot. */
posture.joint_names.resize(2);
posture.joint_names[0] = "panda_finger_joint1";
posture.joint_names[1] = "panda_finger_joint2";
/* Set them as closed. */
posture.points.resize(1);
posture.points[0].positions.resize(2);
posture.points[0].positions[0] = 0.0;
posture.points[0].positions[1] = 0.0;
std::cout << "pos: " << posture.points[0].positions[0] << std::endl;
posture.points[0].effort.resize(2);
posture.points[0].effort[0] = 10;
posture.points[0].effort[1] = 10;
std::cout << "s:" << posture.points[0].effort.size() << std::endl;
posture.points[0].time_from_start = ros::Duration(0.5);
// END_SUB_TUTORIAL
}
void pick(moveit::planning_interface::MoveGroupInterface& move_group)
{
// BEGIN_SUB_TUTORIAL pick1
// Create a vector of grasps to be attempted, currently only creating single grasp.
// This is essentially useful when using a grasp generator to generate and test multiple grasps.
std::vector<moveit_msgs::Grasp> grasps;
grasps.resize(1);
// Setting grasp pose
// ++++++++++++++++++++++
// This is the pose of panda_link8. |br|
// From panda_link8 to the palm of the eef the distance is 0.058, the cube starts 0.01 before 5.0 (half of the length
// of the cube). |br|
// Therefore, the position for panda_link8 = 5 - (length of cube/2 - distance b/w panda_link8 and palm of eef - some
// extra padding)
grasps[0].grasp_pose.header.frame_id = "panda_link0";
/*tf2::Quaternion orientation;
orientation.setRPY(-M_PI / 2, -M_PI / 4, -M_PI / 2);
grasps[0].grasp_pose.pose.orientation = tf2::toMsg(orientation);
grasps[0].grasp_pose.pose.position.x = 0.415;
grasps[0].grasp_pose.pose.position.y = 0;
grasps[0].grasp_pose.pose.position.z = 0.5;*/
grasps[0].grasp_pose.pose = getTargetPickPose();
// Setting pre-grasp approach
// ++++++++++++++++++++++++++
/* Defined with respect to frame_id */
grasps[0].pre_grasp_approach.direction.header.frame_id = "panda_link0";
/* Direction is set as positive x axis */
grasps[0].pre_grasp_approach.direction.vector.z = -1.0;
grasps[0].pre_grasp_approach.min_distance = 0.095;
grasps[0].pre_grasp_approach.desired_distance = 0.115;
// Setting post-grasp retreat
// ++++++++++++++++++++++++++
/* Defined with respect to frame_id */
grasps[0].post_grasp_retreat.direction.header.frame_id = "panda_link0";
/* Direction is set as positive z axis */
grasps[0].post_grasp_retreat.direction.vector.z = 1.0;
grasps[0].post_grasp_retreat.min_distance = 0.1;
grasps[0].post_grasp_retreat.desired_distance = 0.25;
grasps[0].max_contact_force = 12;
//std::vector<std::string> atos {"c_object", "panda_rightfinger", "panda_leftfinger"};
//grasps[0].allowed_touch_objects = atos;
// Setting posture of eef before grasp
// +++++++++++++++++++++++++++++++++++
openGripper(grasps[0].pre_grasp_posture);
// END_SUB_TUTORIAL
// BEGIN_SUB_TUTORIAL pick2
// Setting posture of eef during grasp
// +++++++++++++++++++++++++++++++++++
closedGripper(grasps[0].grasp_posture);
// END_SUB_TUTORIAL
// BEGIN_SUB_TUTORIAL pick3
// Set support surface as table1.
move_group.setSupportSurfaceName("c_table1");
// Call pick to pick up the object using the grasps given
move_group.setMaxVelocityScalingFactor(0.05);
move_group.setMaxAccelerationScalingFactor(0.05);
move_group.pick("c_object", grasps);
//move_group.attachObject("c_object");
// END_SUB_TUTORIAL
}
void place(moveit::planning_interface::MoveGroupInterface& group)
{
// BEGIN_SUB_TUTORIAL place
// TODO(@ridhwanluthra) - Calling place function may lead to "All supplied place locations failed. Retrying last
// location in
// verbose mode." This is a known issue and we are working on fixing it. |br|
// Create a vector of placings to be attempted, currently only creating single place location.
std::vector<moveit_msgs::PlaceLocation> place_location;
place_location.resize(1);
// Setting place location pose
// +++++++++++++++++++++++++++
place_location[0].place_pose.header.frame_id = "panda_link0";
tf2::Quaternion orientation;
orientation.setRPY(0, 0, M_PI / 2);
place_location[0].place_pose.pose.orientation = tf2::toMsg(orientation);
/* While placing it is the exact location of the center of the object. */
place_location[0].place_pose.pose.position.x = 0;
place_location[0].place_pose.pose.position.y = 0.5;
place_location[0].place_pose.pose.position.z = 0.3;
// Setting pre-place approach
// ++++++++++++++++++++++++++
/* Defined with respect to frame_id */
place_location[0].pre_place_approach.direction.header.frame_id = "panda_link0";
/* Direction is set as negative z axis */
place_location[0].pre_place_approach.direction.vector.z = -1.0;
place_location[0].pre_place_approach.min_distance = 0.095;
place_location[0].pre_place_approach.desired_distance = 0.115;
// Setting post-grasp retreat
// ++++++++++++++++++++++++++
/* Defined with respect to frame_id */
place_location[0].post_place_retreat.direction.header.frame_id = "panda_link0";
/* Direction is set as negative y axis */
place_location[0].post_place_retreat.direction.vector.z = 1.0;
place_location[0].post_place_retreat.min_distance = 0.1;
place_location[0].post_place_retreat.desired_distance = 0.25;
// Setting posture of eef after placing object
// +++++++++++++++++++++++++++++++++++++++++++
/* Similar to the pick case */
openGripper(place_location[0].post_place_posture);
// Set support surface as table2.
group.setSupportSurfaceName("c_table2");
// Call place to place the object using the place locations given.
group.setMaxVelocityScalingFactor(0.05);
group.setMaxAccelerationScalingFactor(0.05);
group.place("c_object", place_location);
// END_SUB_TUTORIAL
}
void addCollisionObjects(moveit::planning_interface::PlanningSceneInterface& planning_scene_interface)
{
// BEGIN_SUB_TUTORIAL table1
//
// Creating Environment
// ^^^^^^^^^^^^^^^^^^^^
// Create vector to hold 4 collision objects.
std::vector<moveit_msgs::CollisionObject> collision_objects;
collision_objects.resize(4);
// Add the first table where the cube will originally be kept.
collision_objects[0].id = "c_table1";
collision_objects[0].header.frame_id = "panda_link0";
// Define the primitive and its dimensions.
collision_objects[0].primitives.resize(1);
collision_objects[0].primitives[0].type = collision_objects[0].primitives[0].BOX;
collision_objects[0].primitives[0].dimensions.resize(3);
collision_objects[0].primitives[0].dimensions[0] = 0.2;
collision_objects[0].primitives[0].dimensions[1] = 0.4;
collision_objects[0].primitives[0].dimensions[2] = 0.2;
// Define the pose of the table.
collision_objects[0].primitive_poses.resize(1);
collision_objects[0].primitive_poses[0].position.x = 0.5;
collision_objects[0].primitive_poses[0].position.y = 0;
collision_objects[0].primitive_poses[0].position.z = 0.1;
// END_SUB_TUTORIAL
collision_objects[0].operation = collision_objects[0].ADD;
// BEGIN_SUB_TUTORIAL table2
// Add the second table where we will be placing the cube.
collision_objects[1].id = "c_table2";
collision_objects[1].header.frame_id = "panda_link0";
// Define the primitive and its dimensions.
collision_objects[1].primitives.resize(1);
collision_objects[1].primitives[0].type = collision_objects[1].primitives[0].BOX;
collision_objects[1].primitives[0].dimensions.resize(3);
collision_objects[1].primitives[0].dimensions[0] = 0.4;
collision_objects[1].primitives[0].dimensions[1] = 0.2;
collision_objects[1].primitives[0].dimensions[2] = 0.2;
// Define the pose of the table.
collision_objects[1].primitive_poses.resize(1);
collision_objects[1].primitive_poses[0].position.x = 0;
collision_objects[1].primitive_poses[0].position.y = 0.5;
collision_objects[1].primitive_poses[0].position.z = 0.1;
// END_SUB_TUTORIAL
collision_objects[1].operation = collision_objects[1].ADD;
// BEGIN_SUB_TUTORIAL object
// Define the object that we will be manipulating
collision_objects[2].header.frame_id = "panda_link0";
collision_objects[2].id = "c_object";
//Define the primitive and its dimensions.
collision_objects[2].primitives.resize(1);
collision_objects[2].primitives[0].type = collision_objects[1].primitives[0].BOX;
collision_objects[2].primitives[0].dimensions.resize(3);
collision_objects[2].primitives[0].dimensions[0] = 0.02;
collision_objects[2].primitives[0].dimensions[1] = 0.02;
collision_objects[2].primitives[0].dimensions[2] = 0.2;
//Define the pose of the object.
collision_objects[2].primitive_poses.resize(1);
collision_objects[2].primitive_poses[0].position.x = 0.5;
collision_objects[2].primitive_poses[0].position.y = 0;
collision_objects[2].primitive_poses[0].position.z = 0.3;
geometry_msgs::Pose target_pose;
tf2::Quaternion orientation;
orientation.setRPY(0, 0, 0);
geometry_msgs::Pose object_pose;
collision_objects[2].primitive_poses[0].orientation = tf2::toMsg(orientation);
q_pick_object.w = tf2::toMsg(orientation).w;
q_pick_object.x = tf2::toMsg(orientation).x;
q_pick_object.y = tf2::toMsg(orientation).y;
q_pick_object.z = tf2::toMsg(orientation).z;
// END_SUB_TUTORIAL
// the surface
collision_objects[2].operation = collision_objects[2].ADD;
collision_objects[3].header.frame_id = "panda_link0";
collision_objects[3].id = "surface";
// Define the primitive and its dimensions.
collision_objects[3].primitives.resize(1);
collision_objects[3].primitives[0].type = collision_objects[1].primitives[0].BOX;
collision_objects[3].primitives[0].dimensions.resize(3);
collision_objects[3].primitives[0].dimensions[0] = 3.0;
collision_objects[3].primitives[0].dimensions[1] = 3.0;
collision_objects[3].primitives[0].dimensions[2] = 0.1;
// Define the pose of the object.
collision_objects[3].primitive_poses.resize(1);
collision_objects[3].primitive_poses[0].position.x = 0;
collision_objects[3].primitive_poses[0].position.y = 0;
collision_objects[3].primitive_poses[0].position.z = -0.1;
// END_SUB_TUTORIAL
collision_objects[3].operation = collision_objects[3].ADD;
planning_scene_interface.applyCollisionObjects(collision_objects);
}
int main(int argc, char** argv)
{
ros::init(argc, argv, "panda_arm_pick_place");
ros::NodeHandle nh;
ros::AsyncSpinner spinner(1);
spinner.start();
ROS_INFO_NAMED("constraint_planner", ">>>>>>>>>>>>>>>>> WAITING FOR ROBOT INIT <<<<<<<<<<<<<<<<");
ros::Duration(5.0).sleep();
ROS_INFO_NAMED("constraint_planner", ">>>>>>>>>>>>>>>>> WAKING UP AFTER INIT <<<<<<<<<<<<<<<<");
if(!ros::param::has("max_grasp_approach_velocity"))
{
ros::param::set("max_grasp_approach_velocity", 0.05);
ros::param::set("max_grasp_approach_acceleration", 0.05);
ros::param::set("max_grasp_transition_velocity", 0.05);
ros::param::set("max_grasp_transition_acceleration", 0.05);
}
ros::WallDuration(1.0).sleep();
moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
moveit::planning_interface::MoveGroupInterface group("panda_arm");
group.setPlanningTime(45.0);
addCollisionObjects(planning_scene_interface);
// Wait a bit for ROS things to initialize
ros::WallDuration(1.0).sleep();
pick(group);
ros::WallDuration(1.0).sleep();
place(group);
//std::vector<std::string>objectsToRemove{"object"};
//planning_scene_interface.removeCollisionObjects(objectsToRemove);
ros::waitForShutdown();
return 0;
}
\ No newline at end of file
src/MinimalSimpleMotion.cpp deleted 100644 → 0
+ 0
− 57
View file @ 7ef30876
#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 <trajectory_msgs/JointTrajectoryPoint.h>
/**
* minimal demo of constraint aware planning
*/
int main(int argc, char** argv)
{
ros::init(argc, argv, "SIMPLE PLANNER");
ros::NodeHandle node_handle;
ros::AsyncSpinner spinner(1);
spinner.start();
// wait for robot init of robot_state_initializer
ROS_INFO_NAMED("simple_planner", ">>>>>>>>>>>>>>>>> WAITING FOR ROBOT INIT <<<<<<<<<<<<<<<<");
ros::Duration(5.0).sleep();
ROS_INFO_NAMED("simple_planner", ">>>>>>>>>>>>>>>>> WAKING UP AFTER INIT <<<<<<<<<<<<<<<<");
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);
Eigen::Isometry3d text_pose = Eigen::Isometry3d::Identity();
text_pose.translation().z() = 1.75;
// Getting Basic Information
ROS_INFO_NAMED("simple_planner", "Planning frame: %s", move_group.getPlanningFrame().c_str());
ROS_INFO_NAMED("simple_planner", "End effector link: %s", move_group.getEndEffectorLink().c_str());
ROS_INFO_NAMED("simple_planner", "Available Planning Groups:");
std::copy(move_group.getJointModelGroupNames().begin(), move_group.getJointModelGroupNames().end(),
std::ostream_iterator<std::string>(std::cout, ", "));
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("simple_planner", "Visualizing constraint plan %s", success ? "" : "FAILED");
// Move the simulated robot in gazebo
move_group.move();
ros::shutdown();
return 0;
}
src/SampleConstraintPlanner.cpp → src/ObstacleAwarePlanner.cpp
+ 37
− 37
View file @ 64243a07
#include <moveit/move_group_interface/move_group_interface.h> #include <moveit/move_group_interface/move_group_interface.h>
#include <moveit/planning_scene_interface/planning_scene_interface.h> #include <moveit/planning_scene_interface/planning_scene_interface.h>
#include <moveit/trajectory_processing/iterative_time_parameterization.h>
#include <moveit_msgs/DisplayRobotState.h>
#include <moveit_msgs/DisplayTrajectory.h>
#include <moveit_msgs/AttachedCollisionObject.h>
#include <moveit_msgs/CollisionObject.h> #include <moveit_msgs/CollisionObject.h>
#include <moveit_visual_tools/moveit_visual_tools.h> #include <moveit_visual_tools/moveit_visual_tools.h>
#include <moveit/trajectory_processing/iterative_time_parameterization.h>
#include <trajectory_msgs/JointTrajectoryPoint.h> #include <trajectory_msgs/JointTrajectoryPoint.h>
#include <gazebo_msgs/SpawnModel.h> // service definition for spawning things in gazebo
#include <ros/ros.h>
#include <ros/package.h>
/** /**
* simple demo of constraint aware planning * simple demo of obstacle aware planning
*/ */
int main(int argc, char** argv) int main(int argc, char **argv) {
{ // init the node
ros::init(argc, argv, "CONSTRAINT PLANNER"); ros::init(argc, argv, "obstacle_aware_planner");
ros::NodeHandle node_handle; ros::NodeHandle node_handle;
ros::AsyncSpinner spinner(1); ros::AsyncSpinner spinner(1);
spinner.start(); spinner.start();
// wait for robot init of robot_state_initializer // setup the planning environment
ROS_INFO_NAMED("constraint_planner", ">>>>>>>>>>>>>>>>> WAITING FOR ROBOT INIT <<<<<<<<<<<<<<<<");
ros::Duration(5.0).sleep();
ROS_INFO_NAMED("constraint_planner", ">>>>>>>>>>>>>>>>> WAKING UP AFTER INIT <<<<<<<<<<<<<<<<");
static const std::string PLANNING_GROUP = "panda_arm"; static const std::string PLANNING_GROUP = "panda_arm";
moveit::planning_interface::MoveGroupInterface move_group(PLANNING_GROUP); moveit::planning_interface::MoveGroupInterface move_group(PLANNING_GROUP);
moveit::planning_interface::PlanningSceneInterface planning_scene_interface; moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
const robot_state::JointModelGroup* joint_model_group =
move_group.getCurrentState()->getJointModelGroup(PLANNING_GROUP);
// Visualization Setup // Visualization Setup
// MoveIts visual tools provide capabilities for visualizing objects, robots, and trajectories in RViz
namespace rvt = rviz_visual_tools; namespace rvt = rviz_visual_tools;
moveit_visual_tools::MoveItVisualTools visual_tools("panda_link0"); moveit_visual_tools::MoveItVisualTools visual_tools("panda_link0");
visual_tools.deleteAllMarkers(); 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 // Batch publishing is used to reduce the number of messages being sent to RViz for large visualizations
visual_tools.trigger(); visual_tools.enableBatchPublishing();
// Getting Basic Information // remote control is an introspection tool that allows users to step through a high level script
ROS_INFO_NAMED("constraint_planner", "Planning frame: %s", move_group.getPlanningFrame().c_str()); // via buttons and keyboard shortcuts in RViz
ROS_INFO_NAMED("constraint_planner", "End effector link: %s", move_group.getEndEffectorLink().c_str()); visual_tools.loadRemoteControl();
ROS_INFO_NAMED("constraint_planner", "Available Planning Groups:"); ros::Duration(3.0).sleep();
std::copy(move_group.getJointModelGroupNames().begin(), move_group.getJointModelGroupNames().end(),
std::ostream_iterator<std::string>(std::cout, ", "));
// Define a collision object ROS message. // Define a collision object ROS message.
moveit_msgs::CollisionObject collision_object; moveit_msgs::CollisionObject collision_object;
...@@ -65,12 +54,12 @@ int main(int argc, char** argv) ...@@ -65,12 +54,12 @@ int main(int argc, char** argv)
primitive.dimensions.resize(3); primitive.dimensions.resize(3);
primitive.dimensions[0] = 0.4; primitive.dimensions[0] = 0.4;
primitive.dimensions[1] = 0.1; primitive.dimensions[1] = 0.1;
primitive.dimensions[2] = 0.4; primitive.dimensions[2] = 0.3;
// Define a pose for the box (specified relative to frame_id) // Define a pose for the box (specified relative to frame_id)
geometry_msgs::Pose box_pose; geometry_msgs::Pose box_pose;
box_pose.orientation.w = 1.0; box_pose.orientation.w = 1.0;
box_pose.position.x = 0.4; box_pose.position.x = 0.45;
box_pose.position.y = -0.2; box_pose.position.y = -0.2;
box_pose.position.z = 1.0; box_pose.position.z = 1.0;
...@@ -81,14 +70,23 @@ int main(int argc, char** argv) ...@@ -81,14 +70,23 @@ int main(int argc, char** argv)
std::vector<moveit_msgs::CollisionObject> collision_objects; std::vector<moveit_msgs::CollisionObject> collision_objects;
collision_objects.push_back(collision_object); collision_objects.push_back(collision_object);
// Now, let's add the collision object into the world // We can use visual_tools to wait for user input
ROS_INFO_NAMED("constraint_planner", "Add an object into the world"); visual_tools.trigger();
visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to once the collision object appears in RViz");
// Add the collision object into the world
ROS_INFO("Add an object into the world");
planning_scene_interface.addCollisionObjects(collision_objects); planning_scene_interface.addCollisionObjects(collision_objects);
// Show text in RViz of status shape_msgs::SolidPrimitive shape;
visual_tools.publishText(text_pose, "Add object", rvt::WHITE, rvt::XLARGE); shape.dimensions.resize(3);
shape.dimensions[0] = 0.4;
shape.dimensions[1] = 0.1;
shape.dimensions[2] = 0.3;
shape.type = shape.BOX;
visual_tools.trigger(); visual_tools.trigger();
visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to once the collision object appears in RViz"); visual_tools.prompt("Press 'next' to plan the obstacle aware motion");
// Now when we plan a trajectory it will avoid the obstacle // Now when we plan a trajectory it will avoid the obstacle
move_group.setStartState(*move_group.getCurrentState()); move_group.setStartState(*move_group.getCurrentState());
...@@ -98,11 +96,13 @@ int main(int argc, char** argv) ...@@ -98,11 +96,13 @@ int main(int argc, char** argv)
another_pose.position.y = -0.4; another_pose.position.y = -0.4;
another_pose.position.z = 0.9; another_pose.position.z = 0.9;
move_group.setPoseTarget(another_pose); move_group.setPoseTarget(another_pose);
//move_group.setMaxVelocityScalingFactor(0.5);
move_group.setMaxVelocityScalingFactor(0.5);
move_group.setMaxAccelerationScalingFactor(0.5);
moveit::planning_interface::MoveGroupInterface::Plan my_plan; moveit::planning_interface::MoveGroupInterface::Plan my_plan;
bool success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS); bool success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
ROS_INFO_NAMED("constraint_planner", "Visualizing constraint plan %s", success ? "" : "FAILED"); ROS_INFO_NAMED("obstacle_aware_planner", "Visualizing constraint plan %s", success ? "" : "FAILED");
visual_tools.prompt("Press 'next' to move the simulated robot."); visual_tools.prompt("Press 'next' to move the simulated robot.");
visual_tools.trigger(); visual_tools.trigger();
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment