ur5 example: use properties

This commit is contained in:
Robert Haschke 2017-12-04 10:26:34 +01:00
parent 7d8f8dfc8d
commit 587dcaebb2
9 changed files with 165 additions and 151 deletions

View File

@ -39,29 +39,32 @@ int main(int argc, char** argv){
spawnObject(); spawnObject();
Task t; Task t;
// define global properties used by most stages
t.setProperty("group", std::string("arm"));
t.setProperty("eef", std::string("gripper"));
t.setProperty("planner", std::string("RRTConnectkConfigDefault"));
t.setProperty("link", std::string("s_model_tool0"));
t.add(std::make_unique<stages::CurrentState>("current state")); t.add(std::make_unique<stages::CurrentState>("current state"));
{ {
auto move = std::make_unique<stages::Gripper>("open gripper"); auto move = std::make_unique<stages::Gripper>("open gripper");
move->setEndEffector("gripper"); move->properties().initFrom(PARENT);
move->setTo("open"); move->setTo("open");
t.add(std::move(move)); t.add(std::move(move));
} }
{ {
auto move = std::make_unique<stages::Move>("move to pre-grasp"); auto move = std::make_unique<stages::Move>("move to pre-grasp");
move->setGroup("arm"); move->properties().initFrom(PARENT);
move->setPlannerId("RRTConnectkConfigDefault");
move->setTimeout(8.0); move->setTimeout(8.0);
t.add(std::move(move)); t.add(std::move(move));
} }
{ {
auto move = std::make_unique<stages::CartesianPositionMotion>("proceed to grasp pose"); auto move = std::make_unique<stages::CartesianPositionMotion>("proceed to grasp pose");
move->addSolutionCallback(std::ref(t.introspection())); // move->addSolutionCallback(std::ref(t.introspection()));
move->setGroup("arm"); move->properties().initFrom(PARENT);
move->setLink("s_model_tool0");
move->setMinMaxDistance(.03, 0.1); move->setMinMaxDistance(.03, 0.1);
move->setCartesianStepSize(0.02); move->setCartesianStepSize(0.02);
@ -73,8 +76,7 @@ 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("gripper"); gengrasp->properties().initFrom(PARENT);
//gengrasp->setGroup("arm");
gengrasp->setGripperGraspPose("open"); gengrasp->setGripperGraspPose("open");
gengrasp->setObject("object"); gengrasp->setObject("object");
gengrasp->setGraspOffset(.03); gengrasp->setGraspOffset(.03);
@ -85,7 +87,7 @@ int main(int argc, char** argv){
{ {
auto move = std::make_unique<stages::Gripper>("grasp"); auto move = std::make_unique<stages::Gripper>("grasp");
move->setEndEffector("gripper"); move->properties().initFrom(PARENT);
move->setTo("closed"); move->setTo("closed");
move->graspObject("object"); move->graspObject("object");
t.add(std::move(move)); t.add(std::move(move));
@ -93,8 +95,7 @@ int main(int argc, char** argv){
{ {
auto move = std::make_unique<stages::CartesianPositionMotion>("lift object"); auto move = std::make_unique<stages::CartesianPositionMotion>("lift object");
move->setGroup("arm"); move->properties().initFrom(PARENT);
move->setLink("s_model_tool0");
move->setMinMaxDistance(0.03, 0.05); move->setMinMaxDistance(0.03, 0.05);
move->setCartesianStepSize(0.01); move->setCartesianStepSize(0.01);

View File

@ -34,22 +34,10 @@ public:
void setCartesianStepSize(double distance); void setCartesianStepSize(double distance);
protected: protected:
std::string group_;
std::string link_;
double min_distance_;
double max_distance_;
enum { enum {
MODE_ALONG= 1, MODE_ALONG= 1,
MODE_TOWARDS= 2 MODE_TOWARDS= 2
} mode_; } mode_;
geometry_msgs::PointStamped towards_;
geometry_msgs::Vector3Stamped along_;
double step_size_;
}; };
} } } } } }

View File

@ -32,37 +32,15 @@ public:
void setMaxIKSolutions(uint32_t n); void setMaxIKSolutions(uint32_t n);
void ignoreCollisions(bool flag); void setIgnoreCollisions(bool flag);
protected: protected:
planning_scene::PlanningSceneConstPtr scene_; planning_scene::PlanningSceneConstPtr scene_;
std::string eef_;
std::string group_;
std::string ik_link_;
double grasp_offset_ = 0.0;
uint32_t max_ik_solutions_;
bool ignore_collisions_ = false;
std::string gripper_grasp_pose_;
std::string object_;
double timeout_ = 0.1;
double angle_delta_ = 0.1;
/* temp values */ /* temp values */
double current_angle_ = 0.0; double current_angle_ = 0.0;
double remaining_time_;
bool tried_current_state_as_seed_ = false; bool tried_current_state_as_seed_ = false;
std::vector< std::vector<double> > previous_solutions_; std::vector< std::vector<double> > previous_solutions_;

View File

@ -31,11 +31,6 @@ protected:
robot_trajectory::RobotTrajectoryPtr &trajectory, double &cost, Direction dir); robot_trajectory::RobotTrajectoryPtr &trajectory, double &cost, Direction dir);
protected: protected:
std::string eef_;
std::string named_target_;
std::string grasp_object_;
std::string attach_link_;
planning_pipeline::PlanningPipelinePtr planner_; planning_pipeline::PlanningPipelinePtr planner_;
moveit::planning_interface::MoveGroupInterfacePtr mgi_; moveit::planning_interface::MoveGroupInterfacePtr mgi_;
}; };

View File

@ -4,10 +4,6 @@
#include <moveit/task_constructor/stage.h> #include <moveit/task_constructor/stage.h>
namespace moveit {
namespace planning_interface { MOVEIT_CLASS_FORWARD(MoveGroupInterface)}
}
namespace moveit { namespace task_constructor { namespace stages { namespace moveit { namespace task_constructor { namespace stages {
class Move : public Connecting { class Move : public Connecting {
@ -17,17 +13,12 @@ public:
void init(const planning_scene::PlanningSceneConstPtr &scene); void init(const planning_scene::PlanningSceneConstPtr &scene);
bool compute(const InterfaceState &from, const InterfaceState &to); bool compute(const InterfaceState &from, const InterfaceState &to);
void setGroup(std::string group); void setGroup(const std::__cxx11::string &group);
void setPlannerId(std::string planner); void setPlannerId(const std::__cxx11::string &planner);
void setTimeout(double timeout); void setTimeout(double timeout);
protected: protected:
std::string group_;
std::string planner_id_;
double timeout_;
planning_pipeline::PlanningPipelinePtr planner_; planning_pipeline::PlanningPipelinePtr planner_;
moveit::planning_interface::MoveGroupInterfacePtr mgi_;
}; };
} } } } } }

View File

@ -13,44 +13,51 @@
namespace moveit { namespace task_constructor { namespace stages { namespace moveit { namespace task_constructor { namespace stages {
CartesianPositionMotion::CartesianPositionMotion(std::string name) CartesianPositionMotion::CartesianPositionMotion(std::string name)
: PropagatingEitherWay(name), : PropagatingEitherWay(name)
step_size_(0.005)
{ {
auto& p = properties();
p.declare<std::string>("group", "name of planning group");
p.declare<std::string>("link", "", "name of link used for IK");
p.declare<double>("min_distance", "minimum distance to move");
p.declare<double>("max_distance", "maximum distance to move");
p.declare<double>("step_size", 0.005);
p.declare<geometry_msgs::PointStamped>("towards", "target point of motion");
p.declare<geometry_msgs::Vector3Stamped>("along", "vector along which to move");
} }
void CartesianPositionMotion::setGroup(std::string group){ void CartesianPositionMotion::setGroup(std::string group){
group_= group; setProperty("group", group);
} }
void CartesianPositionMotion::setLink(std::string link){ void CartesianPositionMotion::setLink(std::string link){
link_= link; setProperty("link", link);
} }
void CartesianPositionMotion::setMinDistance(double distance){ void CartesianPositionMotion::setMinDistance(double distance){
min_distance_= distance; setProperty("min_distance", distance);
} }
void CartesianPositionMotion::setMaxDistance(double distance){ void CartesianPositionMotion::setMaxDistance(double distance){
max_distance_= distance; setProperty("max_distance", distance);
} }
void CartesianPositionMotion::setMinMaxDistance(double min_distance, double max_distance){ void CartesianPositionMotion::setMinMaxDistance(double min_distance, double max_distance){
setMinDistance(min_distance); setProperty("min_distance", min_distance);
setMaxDistance(max_distance); setProperty("max_distance", max_distance);
} }
void CartesianPositionMotion::towards(geometry_msgs::PointStamped towards){ void CartesianPositionMotion::towards(geometry_msgs::PointStamped towards){
mode_= CartesianPositionMotion::MODE_TOWARDS; mode_= CartesianPositionMotion::MODE_TOWARDS;
towards_= towards; setProperty("towards", towards);
} }
void CartesianPositionMotion::along(geometry_msgs::Vector3Stamped along){ void CartesianPositionMotion::along(geometry_msgs::Vector3Stamped along){
mode_= CartesianPositionMotion::MODE_ALONG; mode_= CartesianPositionMotion::MODE_ALONG;
along_= along; setProperty("along", along);
} }
void CartesianPositionMotion::setCartesianStepSize(double distance){ void CartesianPositionMotion::setCartesianStepSize(double distance){
step_size_= distance; setProperty("step_size", distance);
} }
namespace { namespace {
@ -68,8 +75,12 @@ bool CartesianPositionMotion::computeForward(const InterfaceState &from){
planning_scene::PlanningScenePtr result_scene = from.scene()->diff(); planning_scene::PlanningScenePtr result_scene = from.scene()->diff();
robot_state::RobotState &robot_state = result_scene->getCurrentStateNonConst(); robot_state::RobotState &robot_state = result_scene->getCurrentStateNonConst();
const moveit::core::JointModelGroup* jmg= robot_state.getJointModelGroup(group_); const auto& props = properties();
const moveit::core::LinkModel* link_model= robot_state.getRobotModel()->getLinkModel(link_); const std::string& group = props.get<std::string>("group");
const std::string& link = props.get<std::string>("link");
const moveit::core::JointModelGroup* jmg= robot_state.getJointModelGroup(group);
const moveit::core::LinkModel* link_model= robot_state.getRobotModel()->getLinkModel(link);
const moveit::core::GroupStateValidityCallbackFn is_valid= const moveit::core::GroupStateValidityCallbackFn is_valid=
std::bind( std::bind(
@ -83,12 +94,12 @@ bool CartesianPositionMotion::computeForward(const InterfaceState &from){
bool succeeded= false; bool succeeded= false;
if( mode_ == CartesianPositionMotion::MODE_TOWARDS ){ if( mode_ == CartesianPositionMotion::MODE_TOWARDS ){
const Eigen::Affine3d& frame= from.scene()->getFrameTransform(towards_.header.frame_id); const geometry_msgs::PointStamped& towards = props.get<geometry_msgs::PointStamped>("towards");
const Eigen::Affine3d& frame= from.scene()->getFrameTransform(towards.header.frame_id);
const Eigen::Affine3d& link_pose= robot_state.getGlobalLinkTransform(link_); const Eigen::Affine3d& link_pose= robot_state.getGlobalLinkTransform(link);
Eigen::Vector3d target_point; Eigen::Vector3d target_point;
tf::pointMsgToEigen(towards_.point, target_point); tf::pointMsgToEigen(towards.point, target_point);
target_point= frame*target_point; target_point= frame*target_point;
// retain orientation of link // retain orientation of link
@ -101,7 +112,7 @@ bool CartesianPositionMotion::computeForward(const InterfaceState &from){
link_model, link_model,
target, target,
true, /* global frame */ true, /* global frame */
step_size_, /* cartesian step size */ props.get<double>("step_size"), /* cartesian step size */
1.5, /* jump threshold */ 1.5, /* jump threshold */
is_valid); is_valid);
@ -109,12 +120,13 @@ bool CartesianPositionMotion::computeForward(const InterfaceState &from){
std::cout << "achieved " << achieved_distance << " of cartesian motion" << std::endl; std::cout << "achieved " << achieved_distance << " of cartesian motion" << std::endl;
succeeded= achieved_distance >= min_distance_; succeeded= achieved_distance >= props.get<double>("min_distance");
} }
else if( mode_ == CartesianPositionMotion::MODE_ALONG ){ else if( mode_ == CartesianPositionMotion::MODE_ALONG ){
const Eigen::Affine3d& frame= robot_state.getGlobalLinkTransform(along_.header.frame_id); const geometry_msgs::Vector3Stamped& along = props.get<geometry_msgs::Vector3Stamped>("along");
const Eigen::Affine3d& frame= robot_state.getGlobalLinkTransform(along.header.frame_id);
Eigen::Vector3d direction; Eigen::Vector3d direction;
tf::vectorMsgToEigen(along_.vector, direction); tf::vectorMsgToEigen(along.vector, direction);
direction= frame.linear()*direction; direction= frame.linear()*direction;
double achieved_distance= robot_state.computeCartesianPath( double achieved_distance= robot_state.computeCartesianPath(
@ -123,14 +135,14 @@ bool CartesianPositionMotion::computeForward(const InterfaceState &from){
link_model, link_model,
direction, direction,
true, /* global frame */ true, /* global frame */
max_distance_, /* distance */ props.get<double>("max_distance"),
step_size_, /* cartesian step size */ props.get<double>("step_size"), /* cartesian step size */
1.5, /* jump threshold */ 1.5, /* jump threshold */
is_valid); is_valid);
std::cout << "achieved " << achieved_distance << " of cartesian motion" << std::endl; std::cout << "achieved " << achieved_distance << " of cartesian motion" << std::endl;
succeeded= achieved_distance >= min_distance_; succeeded= achieved_distance >= props.get<double>("min_distance");
} }
else else
throw std::runtime_error("position motion has neither a goal nor a direction"); throw std::runtime_error("position motion has neither a goal nor a direction");
@ -151,8 +163,12 @@ bool CartesianPositionMotion::computeBackward(const InterfaceState &to){
planning_scene::PlanningScenePtr result_scene = to.scene()->diff(); planning_scene::PlanningScenePtr result_scene = to.scene()->diff();
robot_state::RobotState &robot_state = result_scene->getCurrentStateNonConst(); robot_state::RobotState &robot_state = result_scene->getCurrentStateNonConst();
const moveit::core::JointModelGroup* jmg= robot_state.getJointModelGroup(group_); const auto& props = properties();
const moveit::core::LinkModel* link_model= robot_state.getRobotModel()->getLinkModel(link_); const std::string& group = props.get<std::string>("group");
const std::string& link = props.get<std::string>("link");
const moveit::core::JointModelGroup* jmg= robot_state.getJointModelGroup(group);
const moveit::core::LinkModel* link_model= robot_state.getRobotModel()->getLinkModel(link);
const moveit::core::GroupStateValidityCallbackFn is_valid= const moveit::core::GroupStateValidityCallbackFn is_valid=
std::bind( std::bind(
@ -167,14 +183,15 @@ bool CartesianPositionMotion::computeBackward(const InterfaceState &to){
switch(mode_){ switch(mode_){
case(CartesianPositionMotion::MODE_TOWARDS): case(CartesianPositionMotion::MODE_TOWARDS):
{ {
const Eigen::Affine3d& link_pose= robot_state.getGlobalLinkTransform(link_); const Eigen::Affine3d& link_pose= robot_state.getGlobalLinkTransform(link);
direction= link_pose.linear()*Eigen::Vector3d(-1,0,0); direction= link_pose.linear()*Eigen::Vector3d(-1,0,0);
} }
break; break;
case(CartesianPositionMotion::MODE_ALONG): case(CartesianPositionMotion::MODE_ALONG):
{ {
const Eigen::Affine3d& frame= robot_state.getGlobalLinkTransform(along_.header.frame_id); const geometry_msgs::Vector3Stamped& along = props.get<geometry_msgs::Vector3Stamped>("along");
tf::vectorMsgToEigen(along_.vector, direction); const Eigen::Affine3d& frame= robot_state.getGlobalLinkTransform(along.header.frame_id);
tf::vectorMsgToEigen(along.vector, direction);
direction= frame.linear()*direction; direction= frame.linear()*direction;
} }
break; break;
@ -190,14 +207,14 @@ bool CartesianPositionMotion::computeBackward(const InterfaceState &to){
link_model, link_model,
direction, direction,
true, /* global frame */ true, /* global frame */
max_distance_, /* distance */ props.get<double>("max_distance"),
step_size_, /* cartesian step size */ props.get<double>("step_size"), /* cartesian step size */
1.5, /* jump threshold */ 1.5, /* jump threshold */
is_valid); is_valid);
std::cout << "achieved " << achieved_distance << " of cartesian motion" << std::endl; std::cout << "achieved " << achieved_distance << " of cartesian motion" << std::endl;
bool succeeded= achieved_distance >= min_distance_; bool succeeded= achieved_distance >= props.get<double>("min_distance");
if(succeeded){ if(succeeded){
robot_trajectory::RobotTrajectoryPtr traj= std::make_shared<robot_trajectory::RobotTrajectory>(robot_state.getRobotModel(), jmg); robot_trajectory::RobotTrajectoryPtr traj= std::make_shared<robot_trajectory::RobotTrajectory>(robot_state.getRobotModel(), jmg);

View File

@ -21,6 +21,17 @@ namespace moveit { namespace task_constructor { namespace stages {
GenerateGraspPose::GenerateGraspPose(std::string name) GenerateGraspPose::GenerateGraspPose(std::string name)
: Generator(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>("link", "", "name of link used for IK");
p.declare<std::string>("object");
p.declare<std::string>("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<double>("angle_delta", 0.1, "angular steps (rad)");
p.declare<bool>("ignore_collisions", false);
} }
void GenerateGraspPose::init(const planning_scene::PlanningSceneConstPtr &scene) void GenerateGraspPose::init(const planning_scene::PlanningSceneConstPtr &scene)
@ -30,40 +41,44 @@ void GenerateGraspPose::init(const planning_scene::PlanningSceneConstPtr &scene)
} }
void GenerateGraspPose::setGroup(std::string group){ void GenerateGraspPose::setGroup(std::string group){
group_= group; setProperty("group", group);
} }
void GenerateGraspPose::setLink(std::string ik_link){ void GenerateGraspPose::setLink(std::string ik_link){
ik_link_= ik_link; setProperty("link", ik_link);
} }
void GenerateGraspPose::setEndEffector(std::string eef){ void GenerateGraspPose::setEndEffector(std::string eef){
eef_= eef; setProperty("eef", eef);
} }
void GenerateGraspPose::setGripperGraspPose(std::string pose_name){ void GenerateGraspPose::setGripperGraspPose(std::string pose_name){
gripper_grasp_pose_= pose_name; setProperty("grasp_pose", pose_name);
} }
void GenerateGraspPose::setObject(std::string object){ void GenerateGraspPose::setObject(std::string object){
object_= object; setProperty("object", object);
} }
void GenerateGraspPose::setGraspOffset(double offset){ void GenerateGraspPose::setGraspOffset(double offset){
grasp_offset_= offset; setProperty("grasp_offset", offset);
} }
void GenerateGraspPose::setTimeout(double timeout){ void GenerateGraspPose::setTimeout(double timeout){
timeout_= timeout; setProperty("timeout", timeout);
remaining_time_= timeout;
} }
void GenerateGraspPose::setAngleDelta(double delta){ void GenerateGraspPose::setAngleDelta(double delta){
angle_delta_= delta; setProperty("angle_delta", delta);
} }
void GenerateGraspPose::setMaxIKSolutions(uint32_t n){ void GenerateGraspPose::setMaxIKSolutions(uint32_t n){
max_ik_solutions_= n; setProperty("max_ik_solutions", n);
}
void GenerateGraspPose::setIgnoreCollisions(bool flag)
{
setProperty("ignore_collisions", flag);
} }
bool GenerateGraspPose::canCompute() const{ bool GenerateGraspPose::canCompute() const{
@ -98,48 +113,58 @@ namespace {
} }
bool GenerateGraspPose::compute(){ bool GenerateGraspPose::compute(){
assert(scene_->getRobotModel()->hasEndEffector(eef_) && "The specified end effector is not defined in the srdf"); 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");
planning_scene::PlanningScenePtr grasp_scene = scene_->diff(); planning_scene::PlanningScenePtr grasp_scene = scene_->diff();
robot_state::RobotState &grasp_state = grasp_scene->getCurrentStateNonConst(); robot_state::RobotState &grasp_state = grasp_scene->getCurrentStateNonConst();
const moveit::core::JointModelGroup* jmg_eef= grasp_state.getRobotModel()->getEndEffector(eef_); const moveit::core::JointModelGroup* jmg_eef= grasp_state.getRobotModel()->getEndEffector(eef);
const moveit::core::JointModelGroup* jmg_active= group_.empty() const moveit::core::JointModelGroup* jmg_active= group.empty()
? grasp_state.getJointModelGroup(jmg_eef->getEndEffectorParentGroup().first) ? grasp_state.getJointModelGroup(jmg_eef->getEndEffectorParentGroup().first)
: grasp_state.getJointModelGroup(group_); : grasp_state.getJointModelGroup(group);
const std::string ik_link= ik_link_.empty() ? jmg_eef->getEndEffectorParentGroup().second : ik_link_; std::string link = props.get<std::string>("link");
if (link.empty()) link = jmg_eef->getEndEffectorParentGroup().second;
if(!gripper_grasp_pose_.empty()){ const std::string& grasp_pose_name = props.get<std::string>("grasp_pose");
grasp_state.setToDefaultValues(jmg_eef, gripper_grasp_pose_); if(!grasp_pose_name.empty()){
grasp_state.setToDefaultValues(jmg_eef, grasp_pose_name);
} }
const moveit::core::GroupStateValidityCallbackFn is_valid= const moveit::core::GroupStateValidityCallbackFn is_valid=
std::bind( std::bind(
&isValid, &isValid,
scene_, scene_,
ignore_collisions_, props.get<bool>("ignore_collisions"),
&previous_solutions_, &previous_solutions_,
std::placeholders::_1, std::placeholders::_1,
std::placeholders::_2, std::placeholders::_2,
std::placeholders::_3); std::placeholders::_3);
geometry_msgs::Pose object_pose, grasp_pose; geometry_msgs::Pose object_pose, grasp_pose;
const Eigen::Affine3d object_pose_eigen= scene_->getFrameTransform(object_); const Eigen::Affine3d object_pose_eigen= scene_->getFrameTransform(props.get<std::string>("object"));
if(object_pose_eigen.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); 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() ){ 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)){
std::cout << "computed angle " << current_angle_ std::cout << "computed angle " << current_angle_
<< " with " << previous_solutions_.size() << " with " << previous_solutions_.size()
<< " cached ik solutions" << " cached ik solutions"
<< " and " << remaining_time_ << "s left" << std::endl; << " and " << remaining_time << "s left" << std::endl;
current_angle_+= angle_delta_; current_angle_+= props.get<double>("angle_delta");
remaining_time_= timeout_; remaining_time = props.get<double>("timeout");
tried_current_state_as_seed_= false; tried_current_state_as_seed_= false;
previous_solutions_.clear(); previous_solutions_.clear();
continue; continue;
@ -147,8 +172,8 @@ bool GenerateGraspPose::compute(){
grasp_pose= object_pose; grasp_pose= object_pose;
grasp_pose.position.x-= grasp_offset_*cos(current_angle_); grasp_pose.position.x-= grasp_offset*cos(current_angle_);
grasp_pose.position.y-= grasp_offset_*sin(current_angle_); grasp_pose.position.y-= grasp_offset*sin(current_angle_);
grasp_pose.orientation= tf::createQuaternionMsgFromRollPitchYaw(M_PI, 0.0, 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_)
@ -156,8 +181,8 @@ bool GenerateGraspPose::compute(){
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, ik_link, 1, remaining_time_, is_valid); bool succeeded= grasp_state.setFromIK(jmg_active, grasp_pose, link, 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) {
previous_solutions_.emplace_back(); previous_solutions_.emplace_back();

View File

@ -15,7 +15,13 @@ namespace moveit { namespace task_constructor { namespace stages {
Gripper::Gripper(std::string name) Gripper::Gripper(std::string name)
: PropagatingEitherWay(name) : PropagatingEitherWay(name)
{} {
auto& p = properties();
p.declare<std::string>("eef", "name of end-effector group");
p.declare<std::string>("link", "", "name of link the eef is attached to");
p.declare<std::string>("named_target", "", "named target in eef group");
p.declare<std::string>("grasp_object", "", "name of grasp object");
}
void Gripper::init(const planning_scene::PlanningSceneConstPtr &scene) void Gripper::init(const planning_scene::PlanningSceneConstPtr &scene)
{ {
@ -24,25 +30,25 @@ void Gripper::init(const planning_scene::PlanningSceneConstPtr &scene)
} }
void Gripper::setEndEffector(std::string eef){ void Gripper::setEndEffector(std::string eef){
eef_= eef; setProperty("eef", eef);
} }
void Gripper::setAttachLink(std::string link){ void Gripper::setAttachLink(std::string link){
attach_link_= link; setProperty("link", link);
} }
void Gripper::setFrom(std::string named_target){ void Gripper::setFrom(std::string named_target){
restrictDirection(BACKWARD); restrictDirection(BACKWARD);
named_target_= named_target; setProperty("named_target", named_target);
} }
void Gripper::setTo(std::string named_target){ void Gripper::setTo(std::string named_target){
restrictDirection(FORWARD); restrictDirection(FORWARD);
named_target_= named_target; setProperty("named_target", named_target);
} }
void Gripper::graspObject(std::string grasp_object){ void Gripper::graspObject(std::string grasp_object){
grasp_object_= grasp_object; setProperty("grasp_object", grasp_object);
} }
bool Gripper::compute(const InterfaceState &state, planning_scene::PlanningScenePtr &scene, bool Gripper::compute(const InterfaceState &state, planning_scene::PlanningScenePtr &scene,
@ -50,24 +56,30 @@ bool Gripper::compute(const InterfaceState &state, planning_scene::PlanningScene
scene = state.scene()->diff(); scene = state.scene()->diff();
assert(scene->getRobotModel()); assert(scene->getRobotModel());
const auto& props = properties();
const std::string& eef = props.get<std::string>("eef");
std::string link = props.get<std::string>("link");
const std::string& named_target = props.get<std::string>("named_target");
const std::string& grasp_object = props.get<std::string>("grasp_object");
if(!mgi_){ if(!mgi_){
assert(scene->getRobotModel()->hasEndEffector(eef_) && "no end effector with that name defined in srdf"); assert(scene->getRobotModel()->hasEndEffector(eef) && "no end effector with that name defined in srdf");
const moveit::core::JointModelGroup* jmg= scene->getRobotModel()->getEndEffector(eef_); const moveit::core::JointModelGroup* jmg= scene->getRobotModel()->getEndEffector(eef);
const std::string group_name= jmg->getName(); const std::string group_name= jmg->getName();
mgi_= std::make_shared<moveit::planning_interface::MoveGroupInterface>(group_name); mgi_= std::make_shared<moveit::planning_interface::MoveGroupInterface>(group_name);
if( attach_link_.empty() ){ if( link.empty() ){
attach_link_= jmg->getEndEffectorParentGroup().second; link= jmg->getEndEffectorParentGroup().second;
} }
} }
mgi_->setNamedTarget(named_target_); mgi_->setNamedTarget(named_target);
::planning_interface::MotionPlanRequest req; ::planning_interface::MotionPlanRequest req;
mgi_->constructMotionPlanRequest(req); mgi_->constructMotionPlanRequest(req);
if( !grasp_object_.empty() ){ if( !grasp_object.empty() ){
scene->getAllowedCollisionMatrixNonConst().setEntry(grasp_object_, mgi_->getLinkNames(), true); scene->getAllowedCollisionMatrixNonConst().setEntry(grasp_object, mgi_->getLinkNames(), true);
} }
::planning_interface::MotionPlanResponse res; ::planning_interface::MotionPlanResponse res;
@ -80,10 +92,10 @@ bool Gripper::compute(const InterfaceState &state, planning_scene::PlanningScene
scene->setCurrentState(trajectory->getLastWayPoint()); scene->setCurrentState(trajectory->getLastWayPoint());
// attach object // attach object
if( !grasp_object_.empty() ){ if( !grasp_object.empty() ){
moveit_msgs::AttachedCollisionObject obj; moveit_msgs::AttachedCollisionObject obj;
obj.link_name= attach_link_; obj.link_name= link;
obj.object.id= grasp_object_; obj.object.id= grasp_object;
scene->processAttachedCollisionObjectMsg(obj); scene->processAttachedCollisionObjectMsg(obj);
} }

View File

@ -14,9 +14,13 @@
namespace moveit { namespace task_constructor { namespace stages { namespace moveit { namespace task_constructor { namespace stages {
Move::Move(std::string name) Move::Move(std::string name)
: Connecting(name), : Connecting(name)
timeout_(5.0) {
{} auto& p = properties();
p.declare<double>("timeout", 5.0, "planning timeout");
p.declare<std::string>("group", "name of planning group");
p.declare<std::string>("planner", "", "planner id");
}
void Move::init(const planning_scene::PlanningSceneConstPtr &scene) void Move::init(const planning_scene::PlanningSceneConstPtr &scene)
{ {
@ -24,27 +28,30 @@ void Move::init(const planning_scene::PlanningSceneConstPtr &scene)
planner_ = Task::createPlanner(scene->getRobotModel()); planner_ = Task::createPlanner(scene->getRobotModel());
} }
void Move::setGroup(std::string group){ void Move::setGroup(const std::string& group){
group_= group; setProperty("group", group);
mgi_= std::make_shared<moveit::planning_interface::MoveGroupInterface>(group_);
} }
void Move::setPlannerId(std::string planner){ void Move::setPlannerId(const std::string& planner){
planner_id_= planner; setProperty("planner", planner);
} }
void Move::setTimeout(double timeout){ void Move::setTimeout(double timeout){
timeout_= timeout; setProperty("timeout", timeout);
} }
bool Move::compute(const InterfaceState &from, const InterfaceState &to) { bool Move::compute(const InterfaceState &from, const InterfaceState &to) {
mgi_->setJointValueTarget(to.scene()->getCurrentState()); const auto& props = properties();
if( !planner_id_.empty() ) moveit::planning_interface::MoveGroupInterface mgi(props.get<std::string>("group"));
mgi_->setPlannerId(planner_id_); mgi.setJointValueTarget(to.scene()->getCurrentState());
mgi_->setPlanningTime(timeout_);
const std::string planner_id = props.get<std::string>("planner");
if( !planner_id.empty() )
mgi.setPlannerId(planner_id);
mgi.setPlanningTime(props.get<double>("timeout"));
::planning_interface::MotionPlanRequest req; ::planning_interface::MotionPlanRequest req;
mgi_->constructMotionPlanRequest(req); mgi.constructMotionPlanRequest(req);
ros::Duration(4.0).sleep(); ros::Duration(4.0).sleep();
::planning_interface::MotionPlanResponse res; ::planning_interface::MotionPlanResponse res;