/**
*
* TASK 1: Pick
*
*/
Task t1("pick");
t1.loadRobotModel();
auto sampling_planner = std::make_shared<solvers::PipelinePlanner>();
sampling_planner->setProperty("goal_joint_tolerance", 1e-5); // 1e-5
sampling_planner->setProperty("max_velocity_scaling_factor", 0.5);
sampling_planner->setProperty("max_acceleration_scaling_factor", 0.5);
// Cartesian planner
auto cartesian_planner = std::make_shared<solvers::CartesianPath>();
cartesian_planner->setMaxVelocityScaling(.5);
cartesian_planner->setMaxAccelerationScaling(.5);
cartesian_planner->setStepSize(.002);
t1.setProperty("group", "ur3_arm");
t1.setProperty("eef", "robotiq_2f_140");
t1.setProperty("gripper", "robotiq_2f_140"); // TODO: use this
Stage* current_state= nullptr;
{
auto _current_state = std::make_unique<stages::CurrentState>("current state");
current_state= _current_state.get();
t1.add(std::move(_current_state));
}
// open gripper
{
auto stage = std::make_unique<stages::MoveTo>("open gripper", sampling_planner);
stage->setGoal("open");
stage->setGroup("robotiq_2f_140");
t1.add(std::move(stage));
}
{
// TODO: overload for single planner case
auto stage = std::make_unique<stages::Connect>("move to pre-grasp pose",
stages::Connect::GroupPlannerVector {{"ur3_bio_ik", sampling_planner}});
stage->properties().configureInitFrom(Stage::PARENT); // TODO: convenience-wrapper
t1.add(std::move(stage));
}
{
auto stage = std::make_unique<stages::MoveRelative>("approach object", cartesian_planner);
stage->setMarkerNS("approach");
stage->properties().set("link", "robotiq_2f_140_tcp");
stage->setMinMaxDistance(.0, .75);
stage->setGroup("ur3_arm");
stage->properties().configureInitFrom(Stage::PARENT);
geometry_msgs::Vector3Stamped vec;
vec.header.frame_id = "robotiq_2f_140_tcp";
vec.vector.x = 1;
stage->setDirection(vec);
t1.add(std::move(stage));
}
{
auto stage = std::make_unique<stages::GenerateGraspPose>("grasp workspace pose");
stage->properties().configureInitFrom(Stage::PARENT);
stage->setPreGraspPose("open");
stage->setObject(id_axygen_96_rack);
stage->setAngleDelta(M_PI);
stage->setMonitoredStage(current_state);
auto wrapper = std::make_unique<stages::ComputeIK>("grasp pose", std::move(stage) );
wrapper->setMaxIKSolutions(8);
double gripper_offset = gripper_vertical_offset(0.0778);
// Offset wrt object (Dont need to add support height)
// Align approch with grasp wrt object (orientation)
wrapper->setIKFrame(Eigen::Translation3d(0.044-0.006-gripper_offset,0,0) * Eigen::AngleAxisd(-M_PI / 2.0, Eigen::Vector3d::UnitY()), "robotiq_2f_140_tcp"); // object height - where i want to grip
// TODO adding this will initialize "target_pose" which is internal (or isn't it?)
//wrapper->properties().configureInitFrom(Stage::PARENT);
wrapper->properties().configureInitFrom(Stage::PARENT, {"eef"}); // TODO: convenience wrapper
wrapper->properties().configureInitFrom(Stage::INTERFACE, {"target_pose"});
t1.add(std::move(wrapper));
}
// TODO: encapsulate these three states in stages::Grasp or similar
{
auto stage = std::make_unique<stages::ModifyPlanningScene>("allow gripper->object collision");
stage->allowCollisions(id_axygen_96_rack, t1.getRobotModel()->getJointModelGroup("robotiq_2f_140")->getLinkModelNamesWithCollisionGeometry(), true);
stage->allowCollisions(id_axygen_96_rack, id_support_axygen_96_rack, true);
t1.add(std::move(stage));
}
{
auto stage = std::make_unique<stages::MoveTo>("close gripper", sampling_planner);
stage->setGroup("robotiq_2f_140");
stage->properties().configureInitFrom(Stage::PARENT);
//stage->properties().property("group").configureInitFrom(Stage::PARENT, "robotiq_2f_140"); // TODO this is not convenient
//stage->properties().configureInitFrom(Stage::PARENT); // TODO this is not convenient
//stage->properties().configureInitFrom(Stage::PARENT, {"eef"});
stage->setGoal("close_rack");
t1.add(std::move(stage));
}
Stage* object_grasped= nullptr;
{
auto stage = std::make_unique<stages::ModifyPlanningScene>("attach object");
stage->attachObject(id_axygen_96_rack, "robotiq_2f_140_tcp");
object_grasped= stage.get();
t1.add(std::move(stage));
}
{
auto stage = std::make_unique<stages::MoveRelative>("lift object", cartesian_planner);
stage->properties().configureInitFrom(Stage::PARENT, {"group"});
stage->setMinMaxDistance(.001,.75);
stage->setIKFrame("robotiq_2f_140_tcp"); // TODO property for frame
stage->setMarkerNS("lift");
geometry_msgs::Vector3Stamped vec;
vec.header.frame_id= TABLE_FRAME_LINK; // TODO change from table to object frame
vec.vector.z= 1.0;
stage->setDirection(vec);
t1.add(std::move(stage));
}
t1.enableIntrospection();
ros::NodeHandle nh("~");
bool execute= nh.param<bool>("execute", true);
if(execute){
ROS_INFO("Going to execute first computed solution");
}
ROS_INFO_STREAM( t1 );
// TODO: try { t.validate(); } catch() {}
try {
// TODO: optionally also plan stages if incoming states have infinite cost. This facilitates debugging
t1.plan(nh.param<int>("solutions", 3));
}
catch(InitStageException& e){
ROS_ERROR_STREAM(e);
}
if(!execute || t1.numSolutions() == 0){
std::cout << "waiting for <enter>" << std::endl;
std::cin.get();
}
else {
moveit_task_constructor_msgs::Solution solution;
t1.solutions().front()->fillMessage(solution);
std::cout << "executing solution" << std::endl;
executeSolution(solution);
std::cout << "done" << std::endl;
std::cout << "waiting for <enter>" << std::endl;
std::cin.get();
}
/**
*
* TASK 2: Place
*
*/
Task t2("place");
t2.loadRobotModel();
t2.setProperty("group", "ur3_arm");
t2.setProperty("eef", "robotiq_2f_140");
t2.setProperty("gripper", "robotiq_2f_140"); // TODO: use this
{
auto _current_state = std::make_unique<stages::CurrentState>("current state");
current_state= _current_state.get();
t2.add(std::move(_current_state));
}
{
auto stage = std::make_unique<stages::Connect>("move to pre-place pose", stages::Connect::GroupPlannerVector{{"ur3_bio_ik", sampling_planner}});
stage->setTimeout(15.0);
//stage->setPathConstraints(upright_constraint);
stage->properties().configureInitFrom(Stage::PARENT); // TODO: convenience-wrapper
t2.add(std::move(stage));
}
{
auto stage = std::make_unique<stages::MoveRelative>("put down object", cartesian_planner);
stage->setMarkerNS("approach-place");
stage->properties().set("link", "robotiq_2f_140_tcp");
stage->properties().configureInitFrom(Stage::PARENT, {"group"});
stage->setMinMaxDistance(.0, .75);
geometry_msgs::Vector3Stamped vec;
vec.header.frame_id = "robotiq_2f_140_tcp";
vec.vector.x = 1;
stage->setDirection(vec);
t2.add(std::move(stage));
}
{
auto stage = std::make_unique<stages::GeneratePose>("place pose");
geometry_msgs::PoseStamped p;
tf::poseEigenToMsg(support_axygen_96_rack_pose_1*Eigen::AngleAxisd(0, Eigen::Vector3d::UnitY()), p.pose);
p.header.frame_id= TABLE_FRAME_LINK;
stage->setPose(p);
stage->properties().configureInitFrom(Stage::PARENT);
stage->setMonitoredStage(object_grasped);
auto wrapper = std::make_unique<stages::ComputeIK>("place pose kinematics", std::move(stage) );
wrapper->setMaxIKSolutions(32);
wrapper->setIKFrame(Eigen::Translation3d(0,0,-0.025) * Eigen::AngleAxisd(0, Eigen::Vector3d::UnitY()), id_axygen_96_rack);
wrapper->properties().configureInitFrom(Stage::PARENT, {"eef"}); // TODO: convenience wrapper
wrapper->properties().configureInitFrom(Stage::INTERFACE, {"target_pose"});
t2.add(std::move(wrapper));
}
{
auto stage = std::make_unique<stages::ModifyPlanningScene>("coll allow");
stage->allowCollisions(id_axygen_96_rack, id_support_axygen_96_rack_1, true);
t2.add(std::move(stage));
}
{
auto stage = std::make_unique<stages::MoveRelative>("hack put object in ", cartesian_planner);
stage->properties().configureInitFrom(Stage::PARENT, {"group"});
stage->setIKFrame("robotiq_2f_140_tcp"); // TODO property for frame
stage->setMarkerNS("lol");
geometry_msgs::Vector3Stamped vec;
vec.header.frame_id= TABLE_FRAME_LINK; // TODO change from table to object frame
vec.vector.z= -0.02;
stage->setDirection(vec);
t2.add(std::move(stage));
}
{
auto stage = std::make_unique<stages::MoveTo>("release object", sampling_planner);
stage->setGroup("robotiq_2f_140");
stage->properties().configureInitFrom(Stage::PARENT);
//stage->properties().property("group").configureInitFrom(Stage::PARENT, "robotiq_2f_140"); // TODO this is not convenient
stage->setGoal("open");
t2.add(std::move(stage));
}
{
auto stage = std::make_unique<stages::ModifyPlanningScene>("allow gripper->object collision");
stage->allowCollisions(id_axygen_96_rack, t2.getRobotModel()->getJointModelGroup("robotiq_2f_140")->getLinkModelNamesWithCollisionGeometry(), false);
t2.add(std::move(stage));
}
{
auto stage = std::make_unique<stages::ModifyPlanningScene>("detach object");
stage->detachObject(id_axygen_96_rack, "robotiq_2f_140_tcp");
//object_grasped= stage.get();
t2.add(std::move(stage));
}
{
auto stage = std::make_unique<stages::MoveRelative>("retreat after place", cartesian_planner);
stage->properties().configureInitFrom(Stage::PARENT, {"group"});
stage->setMinMaxDistance(.04,.75);
stage->setIKFrame("robotiq_2f_140_tcp"); // TODO property for frame
stage->setMarkerNS("post-place");
geometry_msgs::Vector3Stamped vec;
vec.header.frame_id= "robotiq_2f_140_tcp";
vec.vector.x= -1.0;
//vec.vector.z= 0.75;
stage->setDirection(vec);
t2.add(std::move(stage));
}
ROS_INFO_STREAM( t2 );
try {
// TODO: optionally also plan stages if incoming states have infinite cost. This facilitates debugging
t2.plan(nh.param<int>("solutions", 3));
}
catch(InitStageException& e){
ROS_ERROR_STREAM(e);
}
if(!execute || t2.numSolutions() == 0){
std::cout << "waiting for <enter>" << std::endl;
std::cin.get();
}
else {
moveit_task_constructor_msgs::Solution solution;
t2.solutions().front()->fillMessage(solution);
std::cout << "executing solution" << std::endl;
executeSolution(solution);
std::cout << "done" << std::endl;
std::cout << "waiting for <enter>" << std::endl;
std::cin.get();
}
return 0;
React
Typescript
Django
Laravel
Facebook
Microsoft
Google
Alibaba
D3
Tencent