# include "mtc_tutorial/mtc_glass_bottle.hpp"
static const rclcpp:: Logger LOGGER = rclcpp:: get_logger ( "mtc_glass_right" ) ;
rclcpp:: node_interfaces:: NodeBaseInterface:: SharedPtr MTCTaskNode_Right :: getNodeBaseInterface ( )
{
return node_-> get_node_base_interface ( ) ;
}
MTCTaskNode_Right :: MTCTaskNode_Right ( const rclcpp:: NodeOptions& options)
: node_{ std:: make_shared < rclcpp:: Node> ( "mtc_right_node" , options) }
{
}
void MTCTaskNode_Right :: setupPlanningScene ( )
{
const double table_height = 1.02 ;
std:: string frame_id = "world" ;
geometry_msgs:: msg:: PoseStamped bottlePose;
bottlePose. header. frame_id = frame_id;
bottlePose. pose. position. x = 0.3 ;
bottlePose. pose. position. y = - 0.5 ;
bottlePose. pose. position. z = table_height;
bottlePose. pose. orientation. w = 1.0 ;
geometry_msgs:: msg:: PoseStamped glassPose;
glassPose. header. frame_id = frame_id;
glassPose. pose. position. x = - 0.6 ;
glassPose. pose. position. y = - 0.5 ;
glassPose. pose. position. z = table_height;
glassPose. pose. orientation. w = 1.0 ;
geometry_msgs:: msg:: PoseStamped tabletopPose;
tabletopPose. header. frame_id = frame_id;
tabletopPose. pose. position. x = 0 ;
tabletopPose. pose. position. y = - 0.25 ;
tabletopPose. pose. position. z = table_height;
tabletopPose. pose. orientation. w = 1.0 ;
mtc_pour:: cleanup ( ) ;
moveit:: planning_interface:: PlanningSceneInterface psi;
std:: vector< moveit_msgs:: msg:: CollisionObject> objs;
mtc_pour:: setupTable ( objs, tabletopPose) ;
mtc_pour:: setupObjects ( objs, bottlePose, glassPose,
"package://mtc_pour/meshes/small_bottle.stl" ) ;
psi. applyCollisionObjects ( objs) ;
}
void MTCTaskNode_Right :: doTask ( )
{
task_ = createTask ( ) ;
try
{
task_. init ( ) ;
}
catch ( mtc:: InitStageException& e)
{
RCLCPP_ERROR_STREAM ( LOGGER, e) ;
return ;
}
if ( ! task_. plan ( 10 ) )
{
RCLCPP_ERROR_STREAM ( LOGGER, "Task planning failed" ) ;
return ;
}
task_. introspection ( ) . publishSolution ( * task_. solutions ( ) . front ( ) ) ;
auto result = task_. execute ( * task_. solutions ( ) . front ( ) ) ;
if ( result. val != moveit_msgs:: msg:: MoveItErrorCodes:: SUCCESS)
{
RCLCPP_ERROR_STREAM ( LOGGER, "Task execution failed" ) ;
return ;
}
return ;
}
mtc:: Task MTCTaskNode_Right :: createTask ( )
{
mtc:: Task task;
task. stages ( ) -> setName ( "grabbing the glass on the right" ) ;
task. loadRobotModel ( node_) ;
const auto & right_arm_group_name = "right_ur_manipulator" ;
const auto & right_hand_group_name = "right_robotiq_2f_85_gripper" ;
const auto & right_hand_frame = "right_grasp_point" ;
task. setProperty ( "group" , right_arm_group_name) ;
task. setProperty ( "eef" , "right_robotiq_2f_85_gripper_ee" ) ;
task. setProperty ( "ik_frame" , right_hand_frame) ;
# pragma GCC diagnostic push
# pragma GCC diagnostic ignored "-Wunused-but-set-variable"
mtc:: Stage* current_state_ptr = nullptr ;
# pragma GCC diagnostic pop
auto stage_state_current = std:: make_unique < mtc:: stages:: CurrentState> ( "right-current" ) ;
current_state_ptr = stage_state_current. get ( ) ;
task. add ( std:: move ( stage_state_current) ) ;
auto sampling_planner = std:: make_shared < mtc:: solvers:: PipelinePlanner> ( node_) ;
auto interpolation_planner = std:: make_shared < mtc:: solvers:: JointInterpolationPlanner> ( ) ;
auto cartesian_planner = std:: make_shared < mtc:: solvers:: CartesianPath> ( ) ;
cartesian_planner-> setMaxVelocityScalingFactor ( 1.0 ) ;
cartesian_planner-> setMaxAccelerationScalingFactor ( 1.0 ) ;
cartesian_planner-> setStepSize ( .01 ) ;
auto stage_open_right_hand =
std:: make_unique < mtc:: stages:: MoveTo> ( "open right_hand" , interpolation_planner) ;
stage_open_right_hand-> setGroup ( right_hand_group_name) ;
stage_open_right_hand-> setGoal ( "right_open" ) ;
task. add ( std:: move ( stage_open_right_hand) ) ;
auto stage_move_to_pick_right = std:: make_unique < mtc:: stages:: Connect> (
"move to pick-right" ,
mtc:: stages:: Connect:: GroupPlannerVector{ { right_arm_group_name, sampling_planner} } ) ;
stage_move_to_pick_right-> setTimeout ( 5.0 ) ;
stage_move_to_pick_right-> properties ( ) . configureInitFrom ( mtc:: Stage:: PARENT) ;
task. add ( std:: move ( stage_move_to_pick_right) ) ;
mtc:: Stage* attach_object_stage_right = nullptr ;
{
auto grasp_right = std:: make_unique < mtc:: SerialContainer> ( "pick glass-right" ) ;
task. properties ( ) . exposeTo ( grasp_right-> properties ( ) , { "eef" , "group" , "ik_frame" } ) ;
grasp_right-> properties ( ) . configureInitFrom ( mtc:: Stage:: PARENT, { "eef" , "group" , "ik_frame" } ) ;
{
auto stage = std:: make_unique < mtc:: stages:: MoveRelative> ( "approach glass-right" , cartesian_planner) ;
stage-> properties ( ) . set ( "marker_ns" , "approach_glass-right" ) ;
stage-> properties ( ) . set ( "link" , right_hand_frame) ;
stage-> properties ( ) . configureInitFrom ( mtc:: Stage:: PARENT, { "group" } ) ;
stage-> setMinMaxDistance ( 0.05 , 0.15 ) ;
geometry_msgs:: msg:: Vector3Stamped vec;
vec. header. frame_id = right_hand_frame;
vec. vector. z = 1.0 ;
stage-> setDirection ( vec) ;
grasp_right-> insert ( std:: move ( stage) ) ;
}
{
auto stage = std:: make_unique < mtc:: stages:: GenerateGraspPose> ( "generate grasp pose -right" ) ;
stage-> properties ( ) . configureInitFrom ( mtc:: Stage:: PARENT) ;
stage-> properties ( ) . set ( "marker_ns" , "grasp_pose-right" ) ;
stage-> setPreGraspPose ( "right_open" ) ;
stage-> setObject ( "glass" ) ;
stage-> setAngleDelta ( M_PI / 12 ) ;
stage-> setMonitoredStage ( current_state_ptr) ;
Eigen:: Isometry3d grasp_frame_transform_right;
Eigen:: Quaterniond q = Eigen :: AngleAxisd ( - M_PI/ 2 , Eigen:: Vector3d :: UnitY ( ) ) *
Eigen :: AngleAxisd ( - M_PI/ 2 , Eigen:: Vector3d :: UnitX ( ) ) ;
grasp_frame_transform_right. linear ( ) = q. matrix ( ) ;
grasp_frame_transform_right. translation ( ) . z ( ) = 0 ;
auto wrapper = std:: make_unique < mtc:: stages:: ComputeIK> ( "grasp pose IK - right" , std:: move ( stage) ) ;
wrapper-> setMaxIKSolutions ( 8 ) ;
wrapper-> setMinSolutionDistance ( 1.0 ) ;
wrapper-> setIKFrame ( grasp_frame_transform_right, right_hand_frame) ;
wrapper-> properties ( ) . configureInitFrom ( mtc:: Stage:: PARENT, { "eef" , "group" } ) ;
wrapper-> properties ( ) . configureInitFrom ( mtc:: Stage:: INTERFACE, { "target_pose" } ) ;
grasp_right-> insert ( std:: move ( wrapper) ) ;
}
{
auto stage =
std:: make_unique < mtc:: stages:: ModifyPlanningScene> ( "allow collision (hand,object) - right" ) ;
stage-> allowCollisions ( "glass" ,
task. getRobotModel ( )
-> getJointModelGroup ( right_hand_group_name)
-> getLinkModelNamesWithCollisionGeometry ( ) ,
true ) ;
grasp_right-> insert ( std:: move ( stage) ) ;
}
{
auto stage = std:: make_unique < mtc:: stages:: MoveTo> ( "close hand-right" , interpolation_planner) ;
stage-> setGroup ( right_hand_group_name) ;
stage-> setGoal ( "right_close" ) ;
grasp_right-> insert ( std:: move ( stage) ) ;
}
{
auto stage = std:: make_unique < mtc:: stages:: ModifyPlanningScene> ( "attcah object - right" ) ;
stage-> attachObject ( "glass" , right_hand_frame) ;
attach_object_stage_right = stage. get ( ) ;
grasp_right-> insert ( std:: move ( stage) ) ;
}
{
auto stage = std:: make_unique < mtc:: stages:: MoveRelative> ( "lift object - right" , cartesian_planner) ;
stage-> properties ( ) . configureInitFrom ( mtc:: Stage:: PARENT, { "group" } ) ;
stage-> setMinMaxDistance ( 0.1 , 0.3 ) ;
stage-> setIKFrame ( right_hand_frame) ;
stage-> properties ( ) . set ( "marker_ns" , "lift_object_right" ) ;
geometry_msgs:: msg:: Vector3Stamped vec;
vec. header. frame_id = "dual_base" ;
vec. vector. y = 1.0 ;
stage-> setDirection ( vec) ;
grasp_right-> insert ( std:: move ( stage) ) ;
}
task. add ( std:: move ( grasp_right) ) ;
}
{
auto stage_move_to_place_right = std:: make_unique < mtc:: stages:: Connect> (
"move to place -right" ,
mtc:: stages:: Connect:: GroupPlannerVector{ { right_arm_group_name, sampling_planner} ,
{ right_hand_group_name, sampling_planner} } ) ;
stage_move_to_place_right-> setTimeout ( 5.0 ) ;
stage_move_to_place_right-> properties ( ) . configureInitFrom ( mtc:: Stage:: PARENT) ;
task. add ( std:: move ( stage_move_to_place_right) ) ;
}
{
auto place_right = std:: make_unique < mtc:: SerialContainer> ( "place object -right" ) ;
task. properties ( ) . exposeTo ( place_right-> properties ( ) , { "eef" , "group" , "ik_frame" } ) ;
place_right-> properties ( ) . configureInitFrom ( mtc:: Stage:: PARENT,
{ "eef" , "group" , "ik_frame" } ) ;
{
auto stage = std:: make_unique < mtc:: stages:: GeneratePlacePose> ( "generate place pose -right" ) ;
stage-> properties ( ) . configureInitFrom ( mtc:: Stage:: PARENT) ;
stage-> properties ( ) . set ( "marker_ns" , "place_pose_right" ) ;
stage-> setObject ( "glass" ) ;
geometry_msgs:: msg:: PoseStamped target_pose_msg_right;
target_pose_msg_right. header. frame_id = "dual_base" ;
target_pose_msg_right. pose. position. x = - 0.3 ;
target_pose_msg_right. pose. position. y = - 0.5 ;
target_pose_msg_right. pose. position. z = 0.3 ;
Eigen:: Quaterniond q_place = Eigen :: AngleAxisd ( - M_PI / 2 , Eigen:: Vector3d :: UnitZ ( ) ) *
Eigen :: AngleAxisd ( 0 , Eigen:: Vector3d :: UnitX ( ) ) ;
Eigen:: Quaterniond q_object = q_place. inverse ( ) ;
tf2:: Quaternion qtn ( q_object. x ( ) , q_object. y ( ) , q_object. z ( ) , q_object. w ( ) ) ;
target_pose_msg_right. pose. orientation = tf2:: toMsg ( qtn) ;
stage-> setPose ( target_pose_msg_right) ;
stage-> setMonitoredStage ( attach_object_stage_right) ;
Eigen:: Isometry3d place_frame_transform_right;
place_frame_transform_right. linear ( ) = q_place. toRotationMatrix ( ) ;
place_frame_transform_right. translation ( ) . z ( ) = 0 ;
auto wrapper = std:: make_unique < mtc:: stages:: ComputeIK> ( "place pose IK - right" , std:: move ( stage) ) ;
wrapper-> setMaxIKSolutions ( 8 ) ;
wrapper-> setMinSolutionDistance ( 1.0 ) ;
wrapper-> setIKFrame ( place_frame_transform_right, right_hand_frame) ;
wrapper-> properties ( ) . configureInitFrom ( mtc:: Stage:: PARENT, { "eef" , "group" } ) ;
wrapper-> properties ( ) . configureInitFrom ( mtc:: Stage:: INTERFACE, { "target_pose" } ) ;
place_right-> insert ( std:: move ( wrapper) ) ;
}
{
auto stage = std:: make_unique < mtc:: stages:: MoveTo> ( "open hand-right" , interpolation_planner) ;
stage-> setGroup ( right_hand_group_name) ;
stage-> setGoal ( "right_open" ) ;
place_right-> insert ( std:: move ( stage) ) ;
}
{
auto stage = std:: make_unique < mtc:: stages:: ModifyPlanningScene> ( "forbid collision (hand,object)-right" ) ;
stage-> allowCollisions ( "glass" ,
task. getRobotModel ( )
-> getJointModelGroup ( right_hand_group_name)
-> getLinkModelNamesWithCollisionGeometry ( ) ,
false ) ;
place_right-> insert ( std:: move ( stage) ) ;
}
{
auto stage = std:: make_unique < mtc:: stages:: ModifyPlanningScene> ( "detach object-right" ) ;
stage-> detachObject ( "glass" , right_hand_frame) ;
place_right-> insert ( std:: move ( stage) ) ;
}
{
auto stage = std:: make_unique < mtc:: stages:: MoveRelative> ( "retreat -right" , cartesian_planner) ;
stage-> properties ( ) . configureInitFrom ( mtc:: Stage:: PARENT, { "group" } ) ;
stage-> setMinMaxDistance ( 0.1 , 0.3 ) ;
stage-> setIKFrame ( right_hand_frame) ;
stage-> properties ( ) . set ( "marker_ns" , "retreat-right" ) ;
geometry_msgs:: msg:: Vector3Stamped vec;
vec. header. frame_id = "world" ;
vec. vector. z= 0.5 ;
stage-> setDirection ( vec) ;
place_right-> insert ( std:: move ( stage) ) ;
}
task. add ( std:: move ( place_right) ) ;
}
{
auto stage = std:: make_unique < mtc:: stages:: MoveTo> ( "return home -right" , interpolation_planner) ;
stage-> properties ( ) . configureInitFrom ( mtc:: Stage:: PARENT, { "group" } ) ;
stage-> setGoal ( "right_home" ) ;
task. add ( std:: move ( stage) ) ;
}
return task;
}