mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
implement IK as separate stage
... wrapping another stage
This commit is contained in:
parent
b6959170db
commit
0918ad6897
@ -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));
|
||||
}
|
||||
|
||||
{
|
||||
|
||||
@ -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));
|
||||
|
||||
@ -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));
|
||||
}
|
||||
|
||||
{
|
||||
|
||||
42
core/include/moveit/task_constructor/stages/compute_ik.h
Normal file
42
core/include/moveit/task_constructor/stages/compute_ik.h
Normal 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_;
|
||||
};
|
||||
|
||||
} } }
|
||||
@ -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_;
|
||||
};
|
||||
|
||||
} } }
|
||||
|
||||
@ -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:
|
||||
|
||||
@ -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
|
||||
|
||||
174
core/src/stages/compute_ik.cpp
Normal file
174
core/src/stages/compute_ik.cpp
Normal 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
|
||||
}
|
||||
}
|
||||
|
||||
} } }
|
||||
@ -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;
|
||||
|
||||
@ -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));
|
||||
|
||||
|
||||
Loading…
Reference in New Issue
Block a user