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

Merge remote-tracking branch 'private/ceti_summer_school'

parents 0834bc9c 97ac3224
No related branches found
No related tags found
No related merge requests found
Show whitespace changes
Inline Side-by-side
Showing
with 590 additions and 148 deletions
CMakeLists.txt
+ 9
− 9
View file @ 9b8c551b
......@@ -53,17 +53,17 @@ catkin_package(CATKIN_DEPENDS
# Specify additional locations of header files Your package locations should be listed before other locations
include_directories(${catkin_INCLUDE_DIRS})
add_executable(SampleConstraintPlanner src/SampleConstraintPlanner.cpp)
add_executable(SampleTimedCartesianPlanner src/SampleTimedCartesianPlanner.cpp)
add_executable(SampleSimpleMotion src/SampleSimpleMotion.cpp)
add_executable(MinimalSimpleMotion src/MinimalSimpleMotion.cpp)
add_executable(BasicCartesianPlanner src/BasicCartesianPlanner.cpp)
add_executable(BasicJointSpacePlanner src/BasicJointSpacePlanner.cpp)
add_executable(ObstacleAwarePlanner src/ObstacleAwarePlanner.cpp)
add_executable(GraspPlanner src/GraspPlanner.cpp)
add_executable(${PROJECT_NAME}_pick_place src/pick_place_tutorial.cpp)
set_target_properties(${PROJECT_NAME}_pick_place PROPERTIES OUTPUT_NAME pick_place PREFIX "")
# Specify libraries to link a library or executable target against
target_link_libraries(SampleConstraintPlanner ${catkin_LIBRARIES})
target_link_libraries(SampleTimedCartesianPlanner ${catkin_LIBRARIES})
target_link_libraries(SampleSimpleMotion ${catkin_LIBRARIES})
target_link_libraries(MinimalSimpleMotion ${catkin_LIBRARIES})
## Add cmake target dependencies of the executable
target_link_libraries(BasicCartesianPlanner ${catkin_LIBRARIES})
target_link_libraries(BasicJointSpacePlanner ${catkin_LIBRARIES})
target_link_libraries(ObstacleAwarePlanner ${catkin_LIBRARIES})
target_link_libraries(GraspPlanner ${catkin_LIBRARIES})
target_link_libraries(${PROJECT_NAME}_pick_place ${catkin_LIBRARIES})
launch/simple_simulation.launch → launch/basic_cartesian_planner.launch
+ 1
− 3
View file @ 9b8c551b
......@@ -4,8 +4,6 @@
<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="SampleSimpleMotion" name="SampleSimpleMotion" />
<node pkg="sample_applications" type="BasicCartesianPlanner" name="BasicCartesianPlannerInstance" respawn="false" output="screen"/>
</launch>
launch/constraint_simulation.launch → launch/basic_joint_space_planner.launch
+ 1
− 3
View file @ 9b8c551b
......@@ -4,8 +4,6 @@
<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="SampleConstraintPlanner" name="SampleConstraintPlanner" />
<node pkg="sample_applications" type="BasicJointSpacePlanner" name="BasicJointSpacePlannerInstance" respawn="false" output="screen"/>
</launch>
launch/grasp_planner.launch 0 → 100644
+ 13
− 0
View file @ 9b8c551b
<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 @ 0834bc9c
<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 @ 9b8c551b
......@@ -4,8 +4,8 @@
<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>
models/box.urdf 0 → 100644
+ 29
− 0
View file @ 9b8c551b
<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
package.xml
+ 2
− 2
View file @ 9b8c551b
<?xml version="1.0"?>
<package format="2">
<name>sample_applications</name>
<version>0.0.0</version>
<version>1.0.0</version>
<description>The sample_applications package</description>
<!-- One maintainer tag required, multiple allowed, one person per tag -->
<!-- Example: -->
<!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
<maintainer email="smt@todo.todo">Chair of Software Technology</maintainer>
<maintainer email="sebastian.ebert@tu-dresden.de">Sebastian Ebert</maintainer>
<!-- One license tag required, multiple allowed, one license per tag -->
......
src/SampleTimedCartesianPlanner.cpp → src/BasicCartesianPlanner.cpp
+ 100
− 0
View file @ 9b8c551b
#include <moveit/move_group_interface/move_group_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>
#include <moveit/trajectory_processing/iterative_time_parameterization.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)
{
ros::init(argc, argv, "sample_timed_cartesian");
int main(int argc, char **argv) {
// init the node
ros::init(argc, argv, "basic_cartesian_planner");
ros::NodeHandle node_handle;
ros::AsyncSpinner spinner(1);
spinner.start();
// wait for robot init of robot_state_initializer
ROS_INFO_NAMED("constraint_planner", ">>>>>>>>>>>>>>>>> WAITING FOR ROBOT INIT <<<<<<<<<<<<<<<<");
ros::Duration(5.0).sleep();
ROS_INFO_NAMED("constraint_planner", ">>>>>>>>>>>>>>>>> WAKING UP AFTER INIT <<<<<<<<<<<<<<<<");
// setup the planning environment
static const std::string PLANNING_GROUP = "panda_arm";
moveit::planning_interface::MoveGroupInterface group(PLANNING_GROUP);
// Visualization Setup
// visualization setup
// MoveIts visual tools provide capabilities for visualizing objects, robots, and trajectories in RViz
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 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
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);
moveit_msgs::DisplayTrajectory display_trajectory;
// we can use visual_tools to wait for user input
visual_tools.prompt("Press 'next' to move the simulated robot. \n");
visual_tools.trigger();
moveit::planning_interface::MoveGroupInterface::Plan plan;
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;
// initial pose of the robot
......@@ -71,46 +69,30 @@ int main(int argc, char **argv)
target_pose_3.position.x = -0.6;
waypoints.push_back(target_pose_3);
moveit_msgs::RobotTrajectory trajectory_msg;
group.setPlanningTime(10.0);
double fraction = group.computeCartesianPath(waypoints,0.01,0.0,trajectory_msg, false);
// The trajectory needs to be modified so it will include velocities as well.
// Create a RobotTrajectory object
robot_trajectory::RobotTrajectory rt(group.getCurrentState()->getRobotModel(), "panda_arm");
// Get a RobotTrajectory from trajectory
rt.setRobotTrajectoryMsg(*group.getCurrentState(), trajectory_msg);
// create a plan object and compute the cartesian motion
moveit::planning_interface::MoveGroupInterface::Plan plan;
moveit_msgs::RobotTrajectory trajectory_msg;
group.computeCartesianPath(waypoints, 0.01, 0.0, trajectory_msg, true);
plan.trajectory_ = 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;
// 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
for(int i = 0; i < trajectory_msg.joint_trajectory.points.size(); i++)
{
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;
}
}
double max_velocity_scaling_factor = 0.1;
double max_acceleration_scaling_factor = 0.1;
for(int j = 0; j < trajectory_msg.joint_trajectory.points.at(i).accelerations.size(); j++)
{
trajectory_msg.joint_trajectory.points.at(i).accelerations.at(j) =
sqrt(std::abs(trajectory_msg.joint_trajectory.points.at(i).accelerations.at(j)));
}
}
iptp.computeTimeStamps(rt, max_velocity_scaling_factor, max_acceleration_scaling_factor);
rt.getRobotTrajectoryMsg(trajectory_msg);
*/
plan.trajectory_ = trajectory_msg;
ROS_INFO("Visualizing plan 4 (cartesian path) (%.2f%% acheived)",fraction * 100.0);
visual_tools.trigger();
visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to move the simulated robot");
// execute the planned motion
group.execute(plan);
ros::shutdown();
......
src/SampleSimpleMotion.cpp → src/BasicJointSpacePlanner.cpp
+ 35
− 30
View file @ 9b8c551b
#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>
......@@ -14,45 +10,45 @@
/**
* simple demo of a robotic motion
*/
int main(int argc, char** argv)
{
ros::init(argc, argv, "SIMPLE PLANNER");
int main(int argc, char **argv) {
// init the node
ros::init(argc, argv, "basic_joint_space_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 <<<<<<<<<<<<<<<<");
// setup the planning environment
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
// visualization setup
// MoveIts visual tools provide capabilities for visualizing objects, robots, and trajectories in RViz
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, "simple_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.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();
// 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:");
// getting basic information
ROS_INFO("Planning frame: %s", move_group.getPlanningFrame().c_str());
ROS_INFO("End effector link: %s", move_group.getEndEffectorLink().c_str());
ROS_INFO("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());
move_group.setStartStateToCurrentState();
// define start and target pose (by specifying the desired pose of the end-effector)
geometry_msgs::Pose another_pose;
another_pose.orientation.w = 1.0;
another_pose.position.x = 0.4;
......@@ -60,14 +56,23 @@ int main(int argc, char** argv)
another_pose.position.z = 0.9;
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;
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.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();
ros::shutdown();
......
src/GraspPlanner.cpp 0 → 100644
+ 352
− 0
View file @ 9b8c551b
// 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 @ 0834bc9c
#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 @ 9b8c551b
#include <moveit/move_group_interface/move_group_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_visual_tools/moveit_visual_tools.h>
#include <moveit/trajectory_processing/iterative_time_parameterization.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)
{
ros::init(argc, argv, "CONSTRAINT PLANNER");
int main(int argc, char **argv) {
// init the node
ros::init(argc, argv, "obstacle_aware_planner");
ros::NodeHandle node_handle;
ros::AsyncSpinner spinner(1);
spinner.start();
// wait for robot init of robot_state_initializer
ROS_INFO_NAMED("constraint_planner", ">>>>>>>>>>>>>>>>> WAITING FOR ROBOT INIT <<<<<<<<<<<<<<<<");
ros::Duration(5.0).sleep();
ROS_INFO_NAMED("constraint_planner", ">>>>>>>>>>>>>>>>> WAKING UP AFTER INIT <<<<<<<<<<<<<<<<");
// setup the planning environment
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
// MoveIts visual tools provide capabilities for visualizing objects, robots, and trajectories in RViz
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();
visual_tools.enableBatchPublishing();
// 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, ", "));
// 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();
// Define a collision object ROS message.
moveit_msgs::CollisionObject collision_object;
......@@ -65,12 +54,12 @@ int main(int argc, char** argv)
primitive.dimensions.resize(3);
primitive.dimensions[0] = 0.4;
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)
geometry_msgs::Pose box_pose;
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.z = 1.0;
......@@ -81,14 +70,23 @@ int main(int argc, char** argv)
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");
// We can use visual_tools to wait for user input
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);
// Show text in RViz of status
visual_tools.publishText(text_pose, "Add object", rvt::WHITE, rvt::XLARGE);
shape_msgs::SolidPrimitive shape;
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.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
move_group.setStartState(*move_group.getCurrentState());
......@@ -98,11 +96,13 @@ int main(int argc, char** argv)
another_pose.position.y = -0.4;
another_pose.position.z = 0.9;
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;
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.trigger();
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment