Compare revisions

# 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)

# 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_executable(BasicCartesianPlanner src/BasicCartesianPlanner.cpp)

add_executable(BasicJointSpacePlanner src/BasicJointSpacePlanner.cpp)

add_executable(ObstacleAwarePlanner src/ObstacleAwarePlanner.cpp)

add_executable(GraspPlanner src/GraspPlanner.cpp)

## 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})

    <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>

    <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>

    <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>

    <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>

<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_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

    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();

#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>

/**

 * 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;

    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();

// 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);