implement IK as separate stage

... wrapping another stage
This commit is contained in:
Robert Haschke 2017-12-14 09:15:29 +01:00
parent b6959170db
commit 0918ad6897
10 changed files with 270 additions and 153 deletions

View File

@ -5,6 +5,7 @@
#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 <moveit/task_constructor/stages/compute_ik.h>
#include <ros/ros.h>
@ -82,8 +83,11 @@ int main(int argc, char** argv){
Eigen::AngleAxisd(-0.5*M_PI, Eigen::Vector3d::UnitY()),
"lh_tool_frame");
gengrasp->setAngleDelta(-.2);
gengrasp->setMaxIKSolutions(1);
t.add(std::move(gengrasp));
auto ik = std::make_unique<stages::ComputeIK>("compute ik", std::move(gengrasp));
ik->properties().configureInitFrom(Stage::PARENT);
ik->setMaxIKSolutions(1);
t.add(std::move(ik));
}
{

View File

@ -5,6 +5,7 @@
#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 <moveit/task_constructor/stages/compute_ik.h>
#include <ros/ros.h>
#include <moveit_msgs/CollisionObject.h>
@ -76,17 +77,20 @@ int main(int argc, char** argv){
{
auto gengrasp= std::make_unique<stages::GenerateGraspPose>("generate grasp pose");
gengrasp->setEndEffector("left_gripper");
//gengrasp->setGroup("arm");
gengrasp->setGripperGraspPose("open");
gengrasp->setObject("object");
gengrasp->setGraspFrame(Eigen::Translation3d(), "l_gripper_tool_frame");
gengrasp->setAngleDelta(.2);
t.add(std::move(gengrasp));
auto ik = std::make_unique<stages::ComputeIK>("compute ik", std::move(gengrasp));
ik->setEndEffector("left_gripper");
t.add(std::move(ik));
}
{
auto move= std::make_unique<stages::Gripper>("grasp");
move->setEndEffector("left_gripper");
move->setAttachLink("l_gripper_tool_frame");
move->setTo("closed");
move->graspObject("object");
t.add(std::move(move));

View File

@ -5,6 +5,7 @@
#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 <moveit/task_constructor/stages/compute_ik.h>
#include <ros/ros.h>
@ -81,8 +82,11 @@ int main(int argc, char** argv){
gengrasp->setObject("object");
gengrasp->setGraspFrame(Eigen::Translation3d(.03,0,0), "s_model_tool0");
gengrasp->setAngleDelta(-.2);
gengrasp->setMaxIKSolutions(8);
t.add(std::move(gengrasp));
auto ik = std::make_unique<stages::ComputeIK>("compute ik", std::move(gengrasp));
ik->properties().configureInitFrom(Stage::PARENT);
ik->setMaxIKSolutions(8);
t.add(std::move(ik));
}
{

View File

@ -0,0 +1,42 @@
//
// Created by llach on 24.11.17.
//
// copyright Michael 'v4hn' Goerner @ 2017
#pragma once
#include <moveit/task_constructor/container.h>
#include <geometry_msgs/PoseStamped.h>
#include <Eigen/Geometry>
namespace moveit {
namespace core { MOVEIT_CLASS_FORWARD(RobotState) }
}
namespace moveit { namespace task_constructor { namespace stages {
class ComputeIK : public Wrapper {
public:
ComputeIK(const std::string &name, pointer &&child = Stage::pointer());
void onNewSolution(const SolutionBase &s) override;
void setTimeout(double timeout);
void setEndEffector(const std::string& eef);
void setTargetPose(const geometry_msgs::PoseStamped &pose);
void setTargetPose(const Eigen::Affine3d& pose, const std::string& link = "");
template <typename T>
void setTargetPose(const T& p, const std::string& link = "") {
Eigen::Affine3d pose; pose = p;
setTargetPose(pose, link);
}
void setMaxIKSolutions(uint32_t n);
void setIgnoreCollisions(bool flag);
protected:
bool tried_current_state_as_seed_ = false;
std::vector< std::vector<double> > previous_solutions_;
};
} } }

View File

@ -51,21 +51,11 @@ public:
bool canCompute() const override;
bool compute() override;
void setEndEffector(std::string eef);
void setGroup(std::string group_name);
void setGripperGraspPose(std::string pose_name);
void setObject(std::string object);
void setTimeout(double timeout);
void setAngleDelta(double delta);
void setMaxIKSolutions(uint32_t n);
void setIgnoreCollisions(bool flag);
void setEndEffector(const std::string &eef);
void setGripperGraspPose(const std::string &pose_name);
void setObject(const std::string &object);
void setGraspFrame(const geometry_msgs::TransformStamped &transform);
void setGraspFrame(const Eigen::Affine3d& transform, const std::string& link = "");
@ -74,17 +64,11 @@ public:
Eigen::Affine3d transform; transform = t;
setGraspFrame(transform, link);
}
void setAngleDelta(double delta);
protected:
planning_scene::PlanningSceneConstPtr scene_;
/* temp values */
planning_scene::PlanningScenePtr scene_;
double current_angle_ = 0.0;
bool tried_current_state_as_seed_ = false;
std::vector< std::vector<double> > previous_solutions_;
};
} } }

View File

@ -49,8 +49,8 @@ public:
void init(const planning_scene::PlanningSceneConstPtr &scene);
bool compute(const InterfaceState &from, const InterfaceState &to);
void setGroup(const std::__cxx11::string &group);
void setPlannerId(const std::__cxx11::string &planner);
void setGroup(const std::string &group);
void setPlannerId(const std::string &planner);
void setTimeout(double timeout);
protected:

View File

@ -1,5 +1,6 @@
add_library(${PROJECT_NAME}_stages
${PROJECT_INCLUDE}/stages/cartesian_position_motion.h
${PROJECT_INCLUDE}/stages/compute_ik.h
${PROJECT_INCLUDE}/stages/current_state.h
${PROJECT_INCLUDE}/stages/generate_grasp_pose.h
${PROJECT_INCLUDE}/stages/gripper.h
@ -7,6 +8,7 @@ add_library(${PROJECT_NAME}_stages
cartesian_position_motion.cpp
current_state.cpp
compute_ik.cpp
generate_grasp_pose.cpp
gripper.cpp
move.cpp

View File

@ -0,0 +1,174 @@
//
// Created by llach on 24.11.17.
//
#include <moveit/task_constructor/stages/compute_ik.h>
#include <moveit/planning_scene/planning_scene.h>
#include <moveit/robot_state/conversions.h>
#include <moveit/robot_state/robot_state.h>
#include <Eigen/Geometry>
#include <eigen_conversions/eigen_msg.h>
#include <chrono>
#include <functional>
#include <ros/console.h>
namespace moveit { namespace task_constructor { namespace stages {
ComputeIK::ComputeIK(const std::string &name, Stage::pointer &&child)
: Wrapper(name, std::move(child))
{
auto& p = properties();
p.declare<double>("timeout", 1.0);
p.declare<std::string>("eef", "name of end-effector group");
p.declare<std::string>("group", "", "name of active group (derived from eef if not provided)");
p.declare<std::string>("default_pose", "", "default joint pose of active group (defines cost of IK)");
p.declare<uint32_t>("max_ik_solutions", 1);
p.declare<bool>("ignore_collisions", false);
// target_pose is read from the interface
p.declare<geometry_msgs::PoseStamped>("target_pose");
p.configureInitFrom(Stage::INTERFACE, {"target_pose"});
}
void ComputeIK::setTimeout(double timeout){
setProperty("timeout", timeout);
}
void ComputeIK::setEndEffector(const std::string &eef){
setProperty("eef", eef);
}
void ComputeIK::setTargetPose(const geometry_msgs::PoseStamped &pose)
{
setProperty("target_pose", pose);
}
void ComputeIK::setTargetPose(const Eigen::Affine3d &pose, const std::string &link)
{
geometry_msgs::PoseStamped pose_msg;
pose_msg.header.frame_id = link;
tf::poseEigenToMsg(pose, pose_msg.pose);
setTargetPose(pose_msg);
}
void ComputeIK::setMaxIKSolutions(uint32_t n){
setProperty("max_ik_solutions", n);
}
void ComputeIK::setIgnoreCollisions(bool flag)
{
setProperty("ignore_collisions", flag);
}
namespace {
bool isValid(planning_scene::PlanningSceneConstPtr scene,
bool ignore_collisions,
std::vector< std::vector<double> >* found_solutions,
robot_state::RobotState* state,
const robot_model::JointModelGroup* jmg,
const double* joint_positions){
for(const auto& sol : *found_solutions ){
if( jmg->distance(joint_positions, sol.data()) < 0.1 )
return false; // to close at already found solutions
}
state->setJointGroupPositions(jmg, joint_positions);
state->update();
found_solutions->emplace_back();
state->copyJointGroupPositions(jmg, found_solutions->back());
if (ignore_collisions)
return true;
if (scene->isStateColliding(const_cast<const robot_state::RobotState&>(*state), jmg->getName()))
return false;
return true;
}
} // anonymous namespace
void ComputeIK::onNewSolution(const SolutionBase &s)
{
assert(s.start() && s.end());
assert(s.start()->scene() == s.end()->scene()); // wrapped child should be a generator
planning_scene::PlanningScenePtr scene = s.start()->scene()->diff();
// enforced initialization from interface ensures that new target_pose is read
properties().performInitFrom(INTERFACE, s.start()->properties(), true);
const auto& props = properties();
const std::string& eef = props.get<std::string>("eef");
assert(scene->getRobotModel()->hasEndEffector(eef) && "The specified end effector is not defined in the srdf");
robot_state::RobotState& robot_state = scene->getCurrentStateNonConst();
const auto& robot_model = robot_state.getRobotModel();
const moveit::core::JointModelGroup* jmg = robot_model->getEndEffector(eef);
const std::string& link_name = jmg->getEndEffectorParentGroup().second;
const std::string& group = props.get<std::string>("group");
jmg = group.empty() ? robot_model->getJointModelGroup(jmg->getEndEffectorParentGroup().first)
: robot_model->getJointModelGroup(group);
// compute target pose w.r.t. link_name
geometry_msgs::PoseStamped target_pose = props.get<geometry_msgs::PoseStamped>("target_pose");
Eigen::Affine3d target_pose_eigen;
tf::poseMsgToEigen(target_pose.pose, target_pose_eigen);
if (!target_pose.header.frame_id.empty() && target_pose.header.frame_id != link_name) {
const Eigen::Affine3d& ref_pose = scene->getFrameTransform(props.get<std::string>(target_pose.header.frame_id));
if(ref_pose.matrix().cwiseEqual(Eigen::Affine3d::Identity().matrix()).all())
ROS_WARN("requested reference frame '%s' for target pose does not exist", target_pose.header.frame_id.c_str());
const Eigen::Affine3d& link_pose = scene->getFrameTransform(link_name);
if(link_pose.matrix().cwiseEqual(Eigen::Affine3d::Identity().matrix()).all())
ROS_WARN("requested link frame '%s' does not exist", link_name.c_str());
// transform target pose such that the link frame will reach there
target_pose_eigen = target_pose_eigen * ref_pose.inverse() * link_pose;
}
// determine joint values of robot pose to compare IK solution with for costs
std::vector<double> compare_pose;
const std::string &compare_pose_name = props.get<std::string>("default_pose");
if (!compare_pose_name.empty()) {
robot_state::RobotState compare_state(robot_state);
compare_state.setToDefaultValues(jmg, compare_pose_name);
compare_state.copyJointGroupPositions(jmg, compare_pose);
} else
robot_state.copyJointGroupPositions(jmg, compare_pose);
const moveit::core::GroupStateValidityCallbackFn is_valid =
std::bind(&isValid,
scene,
props.get<bool>("ignore_collisions"),
&previous_solutions_,
std::placeholders::_1,
std::placeholders::_2,
std::placeholders::_3);
uint32_t max_ik_solutions = props.get<uint32_t>("max_ik_solutions");
tried_current_state_as_seed_= false;
previous_solutions_.clear();
double remaining_time = props.get<double>("timeout");
auto start_time = std::chrono::steady_clock::now();
while (previous_solutions_.size() < max_ik_solutions && remaining_time > 0) {
if(tried_current_state_as_seed_)
robot_state.setToRandomPositions(jmg);
tried_current_state_as_seed_= true;
bool succeeded = robot_state.setFromIK(jmg, target_pose_eigen, link_name, 1, remaining_time, is_valid);
auto now = std::chrono::steady_clock::now();
remaining_time -= std::chrono::duration<double>(now - start_time).count();
start_time = now;
if (succeeded)
spawn(InterfaceState(scene), s.cost() + jmg->distance(previous_solutions_.back().data(), compare_pose.data()));
else if (max_ik_solutions == 1)
break; // first and only attempt failed
}
}
} } }

View File

@ -35,22 +35,11 @@
/* Authors: Michael Goerner */
#include <moveit/task_constructor/stages/generate_grasp_pose.h>
#include <moveit/task_constructor/storage.h>
#include <ros/ros.h>
#include <moveit_msgs/DisplayRobotState.h>
#include <moveit/planning_scene/planning_scene.h>
#include <moveit/robot_state/conversions.h>
#include <moveit/robot_state/robot_state.h>
#include <moveit/move_group_interface/move_group_interface.h>
#include <eigen_conversions/eigen_msg.h>
#include <Eigen/Geometry>
#include <chrono>
#include <functional>
#include <eigen_conversions/eigen_msg.h>
namespace moveit { namespace task_constructor { namespace stages {
@ -58,125 +47,74 @@ GenerateGraspPose::GenerateGraspPose(std::string name)
: Generator(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>("eef_named_pose");
p.declare<std::string>("object");
p.declare<std::string>("eef_grasp_pose");
p.declare<double>("timeout", 0.1);
p.declare<uint32_t>("max_ik_solutions", 1);
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);
}
void GenerateGraspPose::init(const planning_scene::PlanningSceneConstPtr &scene)
{
Generator::init(scene);
scene_ = scene;
scene_ = scene->diff();
}
void GenerateGraspPose::setGroup(std::string group){
setProperty("group", group);
}
void GenerateGraspPose::setEndEffector(std::string eef){
void GenerateGraspPose::setEndEffector(const std::string &eef){
setProperty("eef", eef);
}
void GenerateGraspPose::setGripperGraspPose(std::string pose_name){
setProperty("eef_grasp_pose", pose_name);
void GenerateGraspPose::setGripperGraspPose(const std::string &pose_name){
setProperty("eef_named_pose", pose_name);
}
void GenerateGraspPose::setObject(std::string object){
void GenerateGraspPose::setObject(const std::string &object){
setProperty("object", object);
}
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;
frame.child_frame_id = "grasp_frame";
tf::transformEigenToMsg(transform, frame.transform);
setGraspFrame(frame);
}
void GenerateGraspPose::setTimeout(double timeout){
setProperty("timeout", timeout);
}
void GenerateGraspPose::setAngleDelta(double delta){
setProperty("angle_delta", delta);
}
void GenerateGraspPose::setMaxIKSolutions(uint32_t n){
setProperty("max_ik_solutions", n);
}
void GenerateGraspPose::setIgnoreCollisions(bool flag)
{
setProperty("ignore_collisions", flag);
}
bool GenerateGraspPose::canCompute() const{
return current_angle_ < 2*M_PI && current_angle_ > -2*M_PI;
}
namespace {
bool isValid(planning_scene::PlanningSceneConstPtr scene,
bool ignore_collisions,
std::vector< std::vector<double> >* old_solutions,
robot_state::RobotState* state,
const robot_model::JointModelGroup* jmg,
const double* joint_positions){
for( std::vector<double> sol : *old_solutions ){
if( jmg->distance(joint_positions, sol.data()) < 0.1 ){
return false;
}
}
if(ignore_collisions)
return true;
state->setJointGroupPositions(jmg, joint_positions);
state->update();
if( scene->isStateColliding(const_cast<const robot_state::RobotState&>(*state), jmg->getName()) ){
old_solutions->emplace_back();
state->copyJointGroupPositions(jmg, old_solutions->back());
return false;
}
return true;
}
}
bool GenerateGraspPose::compute(){
const auto& props = properties();
double remaining_time = props.get<double>("timeout");
const std::string& eef = props.get<std::string>("eef");
const std::string& group = props.get<std::string>("group");
assert(scene_->getRobotModel()->hasEndEffector(eef) && "The specified end effector is not defined in the srdf");
const moveit::core::JointModelGroup* jmg = scene_->getRobotModel()->getEndEffector(eef);
planning_scene::PlanningScenePtr grasp_scene = scene_->diff();
robot_state::RobotState &grasp_state = grasp_scene->getCurrentStateNonConst();
const moveit::core::JointModelGroup* jmg_eef= grasp_state.getRobotModel()->getEndEffector(eef);
const moveit::core::JointModelGroup* jmg_active= group.empty()
? grasp_state.getJointModelGroup(jmg_eef->getEndEffectorParentGroup().first)
: grasp_state.getJointModelGroup(group);
robot_state::RobotState &robot_state = scene_->getCurrentStateNonConst();
const std::string& eef_named_pose = props.get<std::string>("eef_named_pose");
if(!eef_named_pose.empty()){
robot_state.setToDefaultValues(jmg , eef_named_pose);
}
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;
const std::string &link_name = jmg ->getEndEffectorParentGroup().second;
if (grasp_frame.header.frame_id.empty()) // if no frame_id is given
grasp_frame.header.frame_id = link_name; // interpret the transform w.r.t. eef link frame
Eigen::Affine3d grasp_pose;
tf::transformMsgToEigen(grasp_frame.transform, grasp_pose);
if (grasp_frame.header.frame_id != link_name) {
// convert grasp_pose to transform relative to link (instead of frame_id)
// convert grasp_pose into transform w.r.t. 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");
@ -187,56 +125,23 @@ bool GenerateGraspPose::compute(){
}
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=
std::bind(
&isValid,
scene_,
props.get<bool>("ignore_collisions"),
&previous_solutions_,
std::placeholders::_1,
std::placeholders::_2,
std::placeholders::_3);
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");
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)){
std::cout << "computed angle " << current_angle_
<< " with " << previous_solutions_.size()
<< " cached ik solutions"
<< " and " << remaining_time << "s left" << std::endl;
current_angle_+= props.get<double>("angle_delta");
remaining_time = props.get<double>("timeout");
tried_current_state_as_seed_= false;
previous_solutions_.clear();
continue;
}
// rotate object pose about z-axis
Eigen::Affine3d goal_pose = object_pose * Eigen::AngleAxisd(current_angle_, Eigen::Vector3d::UnitZ()) * grasp_pose;
current_angle_ += props.get<double>("angle_delta");
if(tried_current_state_as_seed_)
grasp_state.setToRandomPositions(jmg_active);
tried_current_state_as_seed_= true;
InterfaceState state(scene_);
geometry_msgs::PoseStamped goal_pose_msg;
goal_pose_msg.header.frame_id = link_name;
tf::poseEigenToMsg(goal_pose, goal_pose_msg.pose);
state.properties().set("target_pose", goal_pose_msg);
auto now= std::chrono::steady_clock::now();
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) {
previous_solutions_.emplace_back();
grasp_state.copyJointGroupPositions(jmg_active, previous_solutions_.back());
spawn(InterfaceState(grasp_scene));
return true;
}
spawn(std::move(state));
return true;
}
return false;

View File

@ -39,9 +39,7 @@ int main(int argc, char** argv){
auto st= std::make_unique<stages::GenerateGraspPose>("generate grasp candidates");
st->setEndEffector("gripper");
//st->setGroup("arm");
st->setObject("object");
st->setTimeout(0.5);
st->setAngleDelta(0.1);
st->setGraspFrame(Eigen::Translation3d(.03,0,0));