mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
generalize GenerateGraspPose
replacing scalar graspOffset and hard-coded Euler angles with arbitrary graspFrame
This commit is contained in:
parent
587dcaebb2
commit
f2b688d1f0
@ -3,3 +3,6 @@ target_link_libraries(plan_pick_ur5 ${PROJECT_NAME}_stages)
|
|||||||
|
|
||||||
add_executable(plan_pick_trixi plan_pick_trixi.cpp)
|
add_executable(plan_pick_trixi plan_pick_trixi.cpp)
|
||||||
target_link_libraries(plan_pick_trixi ${PROJECT_NAME}_stages)
|
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)
|
||||||
|
|||||||
114
core/demo/plan_pick_pa10.cpp
Normal file
114
core/demo/plan_pick_pa10.cpp
Normal file
@ -0,0 +1,114 @@
|
|||||||
|
#include <moveit/task_constructor/task.h>
|
||||||
|
|
||||||
|
#include <moveit/task_constructor/stages/current_state.h>
|
||||||
|
#include <moveit/task_constructor/stages/gripper.h>
|
||||||
|
#include <moveit/task_constructor/stages/move.h>
|
||||||
|
#include <moveit/task_constructor/stages/generate_grasp_pose.h>
|
||||||
|
#include <moveit/task_constructor/stages/cartesian_position_motion.h>
|
||||||
|
|
||||||
|
#include <ros/ros.h>
|
||||||
|
|
||||||
|
#include <moveit/planning_scene_interface/planning_scene_interface.h>
|
||||||
|
|
||||||
|
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<stages::CurrentState>("current state"));
|
||||||
|
|
||||||
|
{
|
||||||
|
auto move = std::make_unique<stages::Gripper>("open gripper");
|
||||||
|
move->properties().initFrom(PARENT);
|
||||||
|
move->setTo("open");
|
||||||
|
t.add(std::move(move));
|
||||||
|
}
|
||||||
|
|
||||||
|
{
|
||||||
|
auto move = std::make_unique<stages::Move>("move to pre-grasp");
|
||||||
|
move->properties().initFrom(PARENT);
|
||||||
|
move->setTimeout(8.0);
|
||||||
|
t.add(std::move(move));
|
||||||
|
}
|
||||||
|
|
||||||
|
{
|
||||||
|
auto move = std::make_unique<stages::CartesianPositionMotion>("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<stages::GenerateGraspPose>("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<stages::Gripper>("grasp");
|
||||||
|
move->properties().initFrom(PARENT);
|
||||||
|
move->setTo("closed");
|
||||||
|
move->graspObject("object");
|
||||||
|
t.add(std::move(move));
|
||||||
|
}
|
||||||
|
|
||||||
|
{
|
||||||
|
auto move = std::make_unique<stages::CartesianPositionMotion>("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;
|
||||||
|
}
|
||||||
@ -77,10 +77,9 @@ int main(int argc, char** argv){
|
|||||||
auto gengrasp= std::make_unique<stages::GenerateGraspPose>("generate grasp pose");
|
auto gengrasp= std::make_unique<stages::GenerateGraspPose>("generate grasp pose");
|
||||||
gengrasp->setEndEffector("left_gripper");
|
gengrasp->setEndEffector("left_gripper");
|
||||||
//gengrasp->setGroup("arm");
|
//gengrasp->setGroup("arm");
|
||||||
gengrasp->setLink("l_gripper_tool_frame");
|
|
||||||
gengrasp->setGripperGraspPose("open");
|
gengrasp->setGripperGraspPose("open");
|
||||||
gengrasp->setObject("object");
|
gengrasp->setObject("object");
|
||||||
gengrasp->setGraspOffset(.00);
|
gengrasp->setGraspFrame(Eigen::Translation3d(), "l_gripper_tool_frame");
|
||||||
gengrasp->setAngleDelta(.2);
|
gengrasp->setAngleDelta(.2);
|
||||||
t.add(std::move(gengrasp));
|
t.add(std::move(gengrasp));
|
||||||
}
|
}
|
||||||
|
|||||||
@ -79,7 +79,7 @@ int main(int argc, char** argv){
|
|||||||
gengrasp->properties().initFrom(PARENT);
|
gengrasp->properties().initFrom(PARENT);
|
||||||
gengrasp->setGripperGraspPose("open");
|
gengrasp->setGripperGraspPose("open");
|
||||||
gengrasp->setObject("object");
|
gengrasp->setObject("object");
|
||||||
gengrasp->setGraspOffset(.03);
|
gengrasp->setGraspFrame(Eigen::Translation3d(.03,0,0), "s_model_tool0");
|
||||||
gengrasp->setAngleDelta(-.2);
|
gengrasp->setAngleDelta(-.2);
|
||||||
gengrasp->setMaxIKSolutions(8);
|
gengrasp->setMaxIKSolutions(8);
|
||||||
t.add(std::move(gengrasp));
|
t.add(std::move(gengrasp));
|
||||||
|
|||||||
@ -3,6 +3,7 @@
|
|||||||
#pragma once
|
#pragma once
|
||||||
|
|
||||||
#include <moveit/task_constructor/stage.h>
|
#include <moveit/task_constructor/stage.h>
|
||||||
|
#include <Eigen/Geometry>
|
||||||
|
|
||||||
namespace moveit { namespace task_constructor { namespace stages {
|
namespace moveit { namespace task_constructor { namespace stages {
|
||||||
|
|
||||||
@ -18,14 +19,10 @@ public:
|
|||||||
|
|
||||||
void setGroup(std::string group_name);
|
void setGroup(std::string group_name);
|
||||||
|
|
||||||
void setLink(std::string ik_link);
|
|
||||||
|
|
||||||
void setGripperGraspPose(std::string pose_name);
|
void setGripperGraspPose(std::string pose_name);
|
||||||
|
|
||||||
void setObject(std::string object);
|
void setObject(std::string object);
|
||||||
|
|
||||||
void setGraspOffset(double grasp_offset);
|
|
||||||
|
|
||||||
void setTimeout(double timeout);
|
void setTimeout(double timeout);
|
||||||
|
|
||||||
void setAngleDelta(double delta);
|
void setAngleDelta(double delta);
|
||||||
@ -34,6 +31,14 @@ public:
|
|||||||
|
|
||||||
void setIgnoreCollisions(bool flag);
|
void setIgnoreCollisions(bool flag);
|
||||||
|
|
||||||
|
void setGraspFrame(const geometry_msgs::TransformStamped &transform);
|
||||||
|
void setGraspFrame(const Eigen::Affine3d& transform, const std::string& link = "");
|
||||||
|
template <typename T>
|
||||||
|
void setGraspFrame(const T& t, const std::string& link = "") {
|
||||||
|
Eigen::Affine3d transform; transform = t;
|
||||||
|
setGraspFrame(transform, link);
|
||||||
|
}
|
||||||
|
|
||||||
protected:
|
protected:
|
||||||
planning_scene::PlanningSceneConstPtr scene_;
|
planning_scene::PlanningSceneConstPtr scene_;
|
||||||
|
|
||||||
|
|||||||
@ -24,12 +24,11 @@ GenerateGraspPose::GenerateGraspPose(std::string name)
|
|||||||
auto& p = properties();
|
auto& p = properties();
|
||||||
p.declare<std::string>("group", "name of planning group");
|
p.declare<std::string>("group", "name of planning group");
|
||||||
p.declare<std::string>("eef", "name of end-effector group");
|
p.declare<std::string>("eef", "name of end-effector group");
|
||||||
p.declare<std::string>("link", "", "name of link used for IK");
|
|
||||||
p.declare<std::string>("object");
|
p.declare<std::string>("object");
|
||||||
p.declare<std::string>("grasp_pose");
|
p.declare<std::string>("eef_grasp_pose");
|
||||||
p.declare<double>("timeout", 0.1);
|
p.declare<double>("timeout", 0.1);
|
||||||
p.declare<uint32_t>("max_ik_solutions", 1);
|
p.declare<uint32_t>("max_ik_solutions", 1);
|
||||||
p.declare<double>("grasp_offset", 0.0);
|
p.declare<geometry_msgs::TransformStamped>("grasp_frame", geometry_msgs::TransformStamped(), "robot frame to use for grasping");
|
||||||
p.declare<double>("angle_delta", 0.1, "angular steps (rad)");
|
p.declare<double>("angle_delta", 0.1, "angular steps (rad)");
|
||||||
p.declare<bool>("ignore_collisions", false);
|
p.declare<bool>("ignore_collisions", false);
|
||||||
}
|
}
|
||||||
@ -44,24 +43,27 @@ void GenerateGraspPose::setGroup(std::string group){
|
|||||||
setProperty("group", group);
|
setProperty("group", group);
|
||||||
}
|
}
|
||||||
|
|
||||||
void GenerateGraspPose::setLink(std::string ik_link){
|
|
||||||
setProperty("link", ik_link);
|
|
||||||
}
|
|
||||||
|
|
||||||
void GenerateGraspPose::setEndEffector(std::string eef){
|
void GenerateGraspPose::setEndEffector(std::string eef){
|
||||||
setProperty("eef", eef);
|
setProperty("eef", eef);
|
||||||
}
|
}
|
||||||
|
|
||||||
void GenerateGraspPose::setGripperGraspPose(std::string pose_name){
|
void GenerateGraspPose::setGripperGraspPose(std::string pose_name){
|
||||||
setProperty("grasp_pose", pose_name);
|
setProperty("eef_grasp_pose", pose_name);
|
||||||
}
|
}
|
||||||
|
|
||||||
void GenerateGraspPose::setObject(std::string object){
|
void GenerateGraspPose::setObject(std::string object){
|
||||||
setProperty("object", object);
|
setProperty("object", object);
|
||||||
}
|
}
|
||||||
|
|
||||||
void GenerateGraspPose::setGraspOffset(double offset){
|
void GenerateGraspPose::setGraspFrame(const geometry_msgs::TransformStamped &frame){
|
||||||
setProperty("grasp_offset", offset);
|
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){
|
void GenerateGraspPose::setTimeout(double timeout){
|
||||||
@ -130,12 +132,28 @@ bool GenerateGraspPose::compute(){
|
|||||||
? grasp_state.getJointModelGroup(jmg_eef->getEndEffectorParentGroup().first)
|
? grasp_state.getJointModelGroup(jmg_eef->getEndEffectorParentGroup().first)
|
||||||
: grasp_state.getJointModelGroup(group);
|
: grasp_state.getJointModelGroup(group);
|
||||||
|
|
||||||
std::string link = props.get<std::string>("link");
|
geometry_msgs::TransformStamped grasp_frame = props.get<geometry_msgs::TransformStamped>("grasp_frame");
|
||||||
if (link.empty()) link = jmg_eef->getEndEffectorParentGroup().second;
|
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<std::string>("grasp_pose");
|
if (grasp_frame.header.frame_id != link_name) {
|
||||||
if(!grasp_pose_name.empty()){
|
// convert grasp_pose to transform relative to link (instead of frame_id)
|
||||||
grasp_state.setToDefaultValues(jmg_eef, grasp_pose_name);
|
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<std::string>("eef_grasp_pose");
|
||||||
|
if(!eef_grasp_pose.empty()){
|
||||||
|
grasp_state.setToDefaultValues(jmg_eef, eef_grasp_pose);
|
||||||
}
|
}
|
||||||
|
|
||||||
const moveit::core::GroupStateValidityCallbackFn is_valid=
|
const moveit::core::GroupStateValidityCallbackFn is_valid=
|
||||||
@ -148,14 +166,10 @@ bool GenerateGraspPose::compute(){
|
|||||||
std::placeholders::_2,
|
std::placeholders::_2,
|
||||||
std::placeholders::_3);
|
std::placeholders::_3);
|
||||||
|
|
||||||
geometry_msgs::Pose object_pose, grasp_pose;
|
const Eigen::Affine3d object_pose = scene_->getFrameTransform(props.get<std::string>("object"));
|
||||||
const Eigen::Affine3d object_pose_eigen= scene_->getFrameTransform(props.get<std::string>("object"));
|
if(object_pose.matrix().cwiseEqual(Eigen::Affine3d::Identity().matrix()).all())
|
||||||
if(object_pose_eigen.matrix().cwiseEqual(Eigen::Affine3d::Identity().matrix()).all())
|
|
||||||
throw std::runtime_error("requested object does not exist or could not be retrieved");
|
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<double>("grasp_offset");
|
|
||||||
uint32_t max_ik_solutions = props.get<uint32_t>("max_ik_solutions");
|
uint32_t max_ik_solutions = props.get<uint32_t>("max_ik_solutions");
|
||||||
while( canCompute() ){
|
while( canCompute() ){
|
||||||
if( remaining_time <= 0.0 || (max_ik_solutions != 0 && previous_solutions_.size() >= max_ik_solutions)){
|
if( remaining_time <= 0.0 || (max_ik_solutions != 0 && previous_solutions_.size() >= max_ik_solutions)){
|
||||||
@ -170,18 +184,15 @@ bool GenerateGraspPose::compute(){
|
|||||||
continue;
|
continue;
|
||||||
}
|
}
|
||||||
|
|
||||||
grasp_pose= object_pose;
|
// rotate object pose about z-axis
|
||||||
|
Eigen::Affine3d goal_pose = object_pose * Eigen::AngleAxisd(current_angle_, Eigen::Vector3d::UnitZ()) * grasp_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_);
|
|
||||||
|
|
||||||
if(tried_current_state_as_seed_)
|
if(tried_current_state_as_seed_)
|
||||||
grasp_state.setToRandomPositions(jmg_active);
|
grasp_state.setToRandomPositions(jmg_active);
|
||||||
tried_current_state_as_seed_= true;
|
tried_current_state_as_seed_= true;
|
||||||
|
|
||||||
auto now= std::chrono::steady_clock::now();
|
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<double>(std::chrono::steady_clock::now()- now).count();
|
remaining_time-= std::chrono::duration<double>(std::chrono::steady_clock::now()- now).count();
|
||||||
|
|
||||||
if(succeeded) {
|
if(succeeded) {
|
||||||
|
|||||||
@ -43,7 +43,7 @@ int main(int argc, char** argv){
|
|||||||
st->setObject("object");
|
st->setObject("object");
|
||||||
st->setTimeout(0.5);
|
st->setTimeout(0.5);
|
||||||
st->setAngleDelta(0.1);
|
st->setAngleDelta(0.1);
|
||||||
st->setGraspOffset(0.03);
|
st->setGraspFrame(Eigen::Translation3d(.03,0,0));
|
||||||
|
|
||||||
t.add(std::move(st));
|
t.add(std::move(st));
|
||||||
|
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user