diff --git a/core/demo/CMakeLists.txt b/core/demo/CMakeLists.txt index d82dd601..f973c09b 100644 --- a/core/demo/CMakeLists.txt +++ b/core/demo/CMakeLists.txt @@ -3,3 +3,6 @@ target_link_libraries(plan_pick_ur5 ${PROJECT_NAME}_stages) add_executable(plan_pick_trixi plan_pick_trixi.cpp) target_link_libraries(plan_pick_trixi ${PROJECT_NAME}_stages) + +add_executable(plan_pick_pa10 plan_pick_pa10.cpp) +target_link_libraries(plan_pick_pa10 ${PROJECT_NAME}_stages) diff --git a/core/demo/plan_pick_pa10.cpp b/core/demo/plan_pick_pa10.cpp new file mode 100644 index 00000000..16897048 --- /dev/null +++ b/core/demo/plan_pick_pa10.cpp @@ -0,0 +1,114 @@ +#include + +#include +#include +#include +#include +#include + +#include + +#include + +using namespace moveit::task_constructor; + +void spawnObject(){ + moveit::planning_interface::PlanningSceneInterface psi; + + moveit_msgs::CollisionObject o; + o.id= "object"; + o.header.frame_id= "world"; + o.primitive_poses.resize(1); + o.primitive_poses[0].position.x= 0.3; + o.primitive_poses[0].position.y= 0.23; + o.primitive_poses[0].position.z= 0.12; + o.primitive_poses[0].orientation.w= 1.0; + o.primitives.resize(1); + o.primitives[0].type= shape_msgs::SolidPrimitive::CYLINDER; + o.primitives[0].dimensions.resize(2); + o.primitives[0].dimensions[0]= 0.23; + o.primitives[0].dimensions[1]= 0.03; + psi.applyCollisionObject(o); +} + +int main(int argc, char** argv){ + ros::init(argc, argv, "plan_pick"); + ros::AsyncSpinner spinner(1); + spinner.start(); + + spawnObject(); + + Task t; + // define global properties used by most stages + t.setProperty("group", std::string("left_arm")); + t.setProperty("eef", std::string("la_tool_mount")); + t.setProperty("planner", std::string("RRTConnectkConfigDefault")); + t.setProperty("link", std::string("la_tool_mount")); + + t.add(std::make_unique("current state")); + + { + auto move = std::make_unique("open gripper"); + move->properties().initFrom(PARENT); + move->setTo("open"); + t.add(std::move(move)); + } + + { + auto move = std::make_unique("move to pre-grasp"); + move->properties().initFrom(PARENT); + move->setTimeout(8.0); + t.add(std::move(move)); + } + + { + auto move = std::make_unique("proceed to grasp pose"); + move->properties().initFrom(PARENT); + move->setMinMaxDistance(.05, 0.1); + move->setCartesianStepSize(0.02); + + geometry_msgs::PointStamped target; + target.header.frame_id= "object"; + move->towards(target); + t.add(std::move(move)); + } + + { + auto gengrasp = std::make_unique("generate grasp pose"); + gengrasp->properties().initFrom(PARENT); + gengrasp->setGripperGraspPose("open"); + gengrasp->setObject("object"); + gengrasp->setGraspFrame(Eigen::Translation3d(0,0,.05)* + Eigen::AngleAxisd(-0.5*M_PI, Eigen::Vector3d::UnitY()), + "lh_tool_frame"); + gengrasp->setAngleDelta(-.2); + gengrasp->setMaxIKSolutions(1); + t.add(std::move(gengrasp)); + } + + { + auto move = std::make_unique("grasp"); + move->properties().initFrom(PARENT); + move->setTo("closed"); + move->graspObject("object"); + t.add(std::move(move)); + } + + { + auto move = std::make_unique("lift object"); + move->properties().initFrom(PARENT); + move->setMinMaxDistance(0.03, 0.05); + move->setCartesianStepSize(0.01); + + geometry_msgs::Vector3Stamped direction; + direction.header.frame_id= "world"; + direction.vector.z= 1.0; + move->along(direction); + t.add(std::move(move)); + } + + t.plan(); + t.publishAllSolutions(); + + return 0; +} diff --git a/core/demo/plan_pick_trixi.cpp b/core/demo/plan_pick_trixi.cpp index 7c09b158..b385799c 100644 --- a/core/demo/plan_pick_trixi.cpp +++ b/core/demo/plan_pick_trixi.cpp @@ -77,10 +77,9 @@ int main(int argc, char** argv){ auto gengrasp= std::make_unique("generate grasp pose"); gengrasp->setEndEffector("left_gripper"); //gengrasp->setGroup("arm"); - gengrasp->setLink("l_gripper_tool_frame"); gengrasp->setGripperGraspPose("open"); gengrasp->setObject("object"); - gengrasp->setGraspOffset(.00); + gengrasp->setGraspFrame(Eigen::Translation3d(), "l_gripper_tool_frame"); gengrasp->setAngleDelta(.2); t.add(std::move(gengrasp)); } diff --git a/core/demo/plan_pick_ur5.cpp b/core/demo/plan_pick_ur5.cpp index deb7d624..e4f514f7 100644 --- a/core/demo/plan_pick_ur5.cpp +++ b/core/demo/plan_pick_ur5.cpp @@ -79,7 +79,7 @@ int main(int argc, char** argv){ gengrasp->properties().initFrom(PARENT); gengrasp->setGripperGraspPose("open"); gengrasp->setObject("object"); - gengrasp->setGraspOffset(.03); + gengrasp->setGraspFrame(Eigen::Translation3d(.03,0,0), "s_model_tool0"); gengrasp->setAngleDelta(-.2); gengrasp->setMaxIKSolutions(8); t.add(std::move(gengrasp)); diff --git a/core/include/moveit/task_constructor/stages/generate_grasp_pose.h b/core/include/moveit/task_constructor/stages/generate_grasp_pose.h index 2840d33d..e47102d5 100644 --- a/core/include/moveit/task_constructor/stages/generate_grasp_pose.h +++ b/core/include/moveit/task_constructor/stages/generate_grasp_pose.h @@ -3,6 +3,7 @@ #pragma once #include +#include namespace moveit { namespace task_constructor { namespace stages { @@ -18,14 +19,10 @@ public: void setGroup(std::string group_name); - void setLink(std::string ik_link); - void setGripperGraspPose(std::string pose_name); void setObject(std::string object); - void setGraspOffset(double grasp_offset); - void setTimeout(double timeout); void setAngleDelta(double delta); @@ -34,6 +31,14 @@ public: void setIgnoreCollisions(bool flag); + void setGraspFrame(const geometry_msgs::TransformStamped &transform); + void setGraspFrame(const Eigen::Affine3d& transform, const std::string& link = ""); + template + void setGraspFrame(const T& t, const std::string& link = "") { + Eigen::Affine3d transform; transform = t; + setGraspFrame(transform, link); + } + protected: planning_scene::PlanningSceneConstPtr scene_; diff --git a/core/src/stages/generate_grasp_pose.cpp b/core/src/stages/generate_grasp_pose.cpp index e0cbd68e..75acadd9 100644 --- a/core/src/stages/generate_grasp_pose.cpp +++ b/core/src/stages/generate_grasp_pose.cpp @@ -24,12 +24,11 @@ GenerateGraspPose::GenerateGraspPose(std::string name) auto& p = properties(); p.declare("group", "name of planning group"); p.declare("eef", "name of end-effector group"); - p.declare("link", "", "name of link used for IK"); p.declare("object"); - p.declare("grasp_pose"); + p.declare("eef_grasp_pose"); p.declare("timeout", 0.1); p.declare("max_ik_solutions", 1); - p.declare("grasp_offset", 0.0); + p.declare("grasp_frame", geometry_msgs::TransformStamped(), "robot frame to use for grasping"); p.declare("angle_delta", 0.1, "angular steps (rad)"); p.declare("ignore_collisions", false); } @@ -44,24 +43,27 @@ void GenerateGraspPose::setGroup(std::string group){ setProperty("group", group); } -void GenerateGraspPose::setLink(std::string ik_link){ - setProperty("link", ik_link); -} - void GenerateGraspPose::setEndEffector(std::string eef){ setProperty("eef", eef); } void GenerateGraspPose::setGripperGraspPose(std::string pose_name){ - setProperty("grasp_pose", pose_name); + setProperty("eef_grasp_pose", pose_name); } void GenerateGraspPose::setObject(std::string object){ setProperty("object", object); } -void GenerateGraspPose::setGraspOffset(double offset){ - setProperty("grasp_offset", offset); +void GenerateGraspPose::setGraspFrame(const geometry_msgs::TransformStamped &frame){ + setProperty("grasp_frame", frame); +} +void GenerateGraspPose::setGraspFrame(const Eigen::Affine3d &transform, const std::string &link) +{ + geometry_msgs::TransformStamped frame; + frame.header.frame_id = link; + tf::transformEigenToMsg(transform, frame.transform); + setGraspFrame(frame); } void GenerateGraspPose::setTimeout(double timeout){ @@ -130,12 +132,28 @@ bool GenerateGraspPose::compute(){ ? grasp_state.getJointModelGroup(jmg_eef->getEndEffectorParentGroup().first) : grasp_state.getJointModelGroup(group); - std::string link = props.get("link"); - if (link.empty()) link = jmg_eef->getEndEffectorParentGroup().second; + geometry_msgs::TransformStamped grasp_frame = props.get("grasp_frame"); + const std::string &link_name = jmg_eef->getEndEffectorParentGroup().second; + if (grasp_frame.header.frame_id.empty()) + grasp_frame.header.frame_id = link_name; + Eigen::Affine3d grasp_pose; + tf::transformMsgToEigen(grasp_frame.transform, grasp_pose); - const std::string& grasp_pose_name = props.get("grasp_pose"); - if(!grasp_pose_name.empty()){ - grasp_state.setToDefaultValues(jmg_eef, grasp_pose_name); + if (grasp_frame.header.frame_id != link_name) { + // convert grasp_pose to transform relative to link (instead of frame_id) + const Eigen::Affine3d link_pose = scene_->getFrameTransform(link_name); + if(link_pose.matrix().cwiseEqual(Eigen::Affine3d::Identity().matrix()).all()) + throw std::runtime_error("requested link does not exist or could not be retrieved"); + const Eigen::Affine3d frame_pose = scene_->getFrameTransform(grasp_frame.header.frame_id); + if(frame_pose.matrix().cwiseEqual(Eigen::Affine3d::Identity().matrix()).all()) + throw std::runtime_error("requested frame does not exist or could not be retrieved"); + grasp_pose = link_pose.inverse() * frame_pose * grasp_pose; + } + grasp_pose = grasp_pose.inverse(); // invert once + + const std::string& eef_grasp_pose = props.get("eef_grasp_pose"); + if(!eef_grasp_pose.empty()){ + grasp_state.setToDefaultValues(jmg_eef, eef_grasp_pose); } const moveit::core::GroupStateValidityCallbackFn is_valid= @@ -148,14 +166,10 @@ bool GenerateGraspPose::compute(){ std::placeholders::_2, std::placeholders::_3); - geometry_msgs::Pose object_pose, grasp_pose; - const Eigen::Affine3d object_pose_eigen= scene_->getFrameTransform(props.get("object")); - if(object_pose_eigen.matrix().cwiseEqual(Eigen::Affine3d::Identity().matrix()).all()) + const Eigen::Affine3d object_pose = scene_->getFrameTransform(props.get("object")); + if(object_pose.matrix().cwiseEqual(Eigen::Affine3d::Identity().matrix()).all()) throw std::runtime_error("requested object does not exist or could not be retrieved"); - tf::poseEigenToMsg(object_pose_eigen, object_pose); - - double grasp_offset = props.get("grasp_offset"); uint32_t max_ik_solutions = props.get("max_ik_solutions"); while( canCompute() ){ if( remaining_time <= 0.0 || (max_ik_solutions != 0 && previous_solutions_.size() >= max_ik_solutions)){ @@ -170,18 +184,15 @@ bool GenerateGraspPose::compute(){ continue; } - grasp_pose= object_pose; - - grasp_pose.position.x-= grasp_offset*cos(current_angle_); - grasp_pose.position.y-= grasp_offset*sin(current_angle_); - grasp_pose.orientation= tf::createQuaternionMsgFromRollPitchYaw(M_PI, 0.0, current_angle_); + // rotate object pose about z-axis + Eigen::Affine3d goal_pose = object_pose * Eigen::AngleAxisd(current_angle_, Eigen::Vector3d::UnitZ()) * grasp_pose; if(tried_current_state_as_seed_) grasp_state.setToRandomPositions(jmg_active); tried_current_state_as_seed_= true; auto now= std::chrono::steady_clock::now(); - bool succeeded= grasp_state.setFromIK(jmg_active, grasp_pose, link, 1, remaining_time, is_valid); + bool succeeded= grasp_state.setFromIK(jmg_active, goal_pose, link_name, 1, remaining_time, is_valid); remaining_time-= std::chrono::duration(std::chrono::steady_clock::now()- now).count(); if(succeeded) { diff --git a/core/test/test_plan_generate_grasp_pose.cpp b/core/test/test_plan_generate_grasp_pose.cpp index d3040bcf..069a7f60 100644 --- a/core/test/test_plan_generate_grasp_pose.cpp +++ b/core/test/test_plan_generate_grasp_pose.cpp @@ -43,7 +43,7 @@ int main(int argc, char** argv){ st->setObject("object"); st->setTimeout(0.5); st->setAngleDelta(0.1); - st->setGraspOffset(0.03); + st->setGraspFrame(Eigen::Translation3d(.03,0,0)); t.add(std::move(st));