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

updated applications

parent 7ef30876
No related branches found
No related tags found
No related merge requests found
...@@ -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})
...@@ -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>
...@@ -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>
<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>
<include file="$(find panda_simulation)/launch/simulation.launch"/>
<node pkg="sample_applications" type="MinimalSimpleMotion" name="MinimalSimpleMotion" />
</launch>
...@@ -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>
<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
#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();
......
#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();
......
// 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
#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;
}
#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% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment