generalize GenerateGraspPose

replacing scalar graspOffset and hard-coded Euler angles with arbitrary graspFrame
This commit is contained in:
Robert Haschke 2017-12-12 15:02:17 +01:00
parent 587dcaebb2
commit f2b688d1f0
7 changed files with 167 additions and 35 deletions

View File

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

View 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;
}

View File

@ -77,10 +77,9 @@ int main(int argc, char** argv){
auto gengrasp= std::make_unique<stages::GenerateGraspPose>("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));
}

View File

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

View File

@ -3,6 +3,7 @@
#pragma once
#include <moveit/task_constructor/stage.h>
#include <Eigen/Geometry>
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 <typename T>
void setGraspFrame(const T& t, const std::string& link = "") {
Eigen::Affine3d transform; transform = t;
setGraspFrame(transform, link);
}
protected:
planning_scene::PlanningSceneConstPtr scene_;

View File

@ -24,12 +24,11 @@ GenerateGraspPose::GenerateGraspPose(std::string name)
auto& p = properties();
p.declare<std::string>("group", "name of planning 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>("grasp_pose");
p.declare<std::string>("eef_grasp_pose");
p.declare<double>("timeout", 0.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<bool>("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<std::string>("link");
if (link.empty()) link = jmg_eef->getEndEffectorParentGroup().second;
geometry_msgs::TransformStamped grasp_frame = props.get<geometry_msgs::TransformStamped>("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<std::string>("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<std::string>("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<std::string>("object"));
if(object_pose_eigen.matrix().cwiseEqual(Eigen::Affine3d::Identity().matrix()).all())
const Eigen::Affine3d object_pose = scene_->getFrameTransform(props.get<std::string>("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<double>("grasp_offset");
uint32_t max_ik_solutions = props.get<uint32_t>("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<double>(std::chrono::steady_clock::now()- now).count();
if(succeeded) {

View File

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