mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
ur5 example: use properties
This commit is contained in:
parent
7d8f8dfc8d
commit
587dcaebb2
@ -39,29 +39,32 @@ int main(int argc, char** argv){
|
||||
spawnObject();
|
||||
|
||||
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"));
|
||||
|
||||
{
|
||||
auto move = std::make_unique<stages::Gripper>("open gripper");
|
||||
move->setEndEffector("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->setGroup("arm");
|
||||
move->setPlannerId("RRTConnectkConfigDefault");
|
||||
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->addSolutionCallback(std::ref(t.introspection()));
|
||||
move->setGroup("arm");
|
||||
move->setLink("s_model_tool0");
|
||||
// move->addSolutionCallback(std::ref(t.introspection()));
|
||||
move->properties().initFrom(PARENT);
|
||||
move->setMinMaxDistance(.03, 0.1);
|
||||
move->setCartesianStepSize(0.02);
|
||||
|
||||
@ -73,8 +76,7 @@ int main(int argc, char** argv){
|
||||
|
||||
{
|
||||
auto gengrasp = std::make_unique<stages::GenerateGraspPose>("generate grasp pose");
|
||||
gengrasp->setEndEffector("gripper");
|
||||
//gengrasp->setGroup("arm");
|
||||
gengrasp->properties().initFrom(PARENT);
|
||||
gengrasp->setGripperGraspPose("open");
|
||||
gengrasp->setObject("object");
|
||||
gengrasp->setGraspOffset(.03);
|
||||
@ -85,7 +87,7 @@ int main(int argc, char** argv){
|
||||
|
||||
{
|
||||
auto move = std::make_unique<stages::Gripper>("grasp");
|
||||
move->setEndEffector("gripper");
|
||||
move->properties().initFrom(PARENT);
|
||||
move->setTo("closed");
|
||||
move->graspObject("object");
|
||||
t.add(std::move(move));
|
||||
@ -93,8 +95,7 @@ int main(int argc, char** argv){
|
||||
|
||||
{
|
||||
auto move = std::make_unique<stages::CartesianPositionMotion>("lift object");
|
||||
move->setGroup("arm");
|
||||
move->setLink("s_model_tool0");
|
||||
move->properties().initFrom(PARENT);
|
||||
move->setMinMaxDistance(0.03, 0.05);
|
||||
move->setCartesianStepSize(0.01);
|
||||
|
||||
|
||||
@ -34,22 +34,10 @@ public:
|
||||
void setCartesianStepSize(double distance);
|
||||
|
||||
protected:
|
||||
std::string group_;
|
||||
|
||||
std::string link_;
|
||||
|
||||
double min_distance_;
|
||||
double max_distance_;
|
||||
|
||||
enum {
|
||||
MODE_ALONG= 1,
|
||||
MODE_TOWARDS= 2
|
||||
} mode_;
|
||||
|
||||
geometry_msgs::PointStamped towards_;
|
||||
geometry_msgs::Vector3Stamped along_;
|
||||
|
||||
double step_size_;
|
||||
};
|
||||
|
||||
} } }
|
||||
|
||||
@ -32,37 +32,15 @@ public:
|
||||
|
||||
void setMaxIKSolutions(uint32_t n);
|
||||
|
||||
void ignoreCollisions(bool flag);
|
||||
void setIgnoreCollisions(bool flag);
|
||||
|
||||
protected:
|
||||
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 */
|
||||
|
||||
double current_angle_ = 0.0;
|
||||
|
||||
double remaining_time_;
|
||||
|
||||
bool tried_current_state_as_seed_ = false;
|
||||
|
||||
std::vector< std::vector<double> > previous_solutions_;
|
||||
|
||||
@ -31,11 +31,6 @@ protected:
|
||||
robot_trajectory::RobotTrajectoryPtr &trajectory, double &cost, Direction dir);
|
||||
|
||||
protected:
|
||||
std::string eef_;
|
||||
std::string named_target_;
|
||||
std::string grasp_object_;
|
||||
std::string attach_link_;
|
||||
|
||||
planning_pipeline::PlanningPipelinePtr planner_;
|
||||
moveit::planning_interface::MoveGroupInterfacePtr mgi_;
|
||||
};
|
||||
|
||||
@ -4,10 +4,6 @@
|
||||
|
||||
#include <moveit/task_constructor/stage.h>
|
||||
|
||||
namespace moveit {
|
||||
namespace planning_interface { MOVEIT_CLASS_FORWARD(MoveGroupInterface)}
|
||||
}
|
||||
|
||||
namespace moveit { namespace task_constructor { namespace stages {
|
||||
|
||||
class Move : public Connecting {
|
||||
@ -17,17 +13,12 @@ public:
|
||||
void init(const planning_scene::PlanningSceneConstPtr &scene);
|
||||
bool compute(const InterfaceState &from, const InterfaceState &to);
|
||||
|
||||
void setGroup(std::string group);
|
||||
void setPlannerId(std::string planner);
|
||||
void setGroup(const std::__cxx11::string &group);
|
||||
void setPlannerId(const std::__cxx11::string &planner);
|
||||
void setTimeout(double timeout);
|
||||
|
||||
protected:
|
||||
std::string group_;
|
||||
std::string planner_id_;
|
||||
double timeout_;
|
||||
|
||||
planning_pipeline::PlanningPipelinePtr planner_;
|
||||
moveit::planning_interface::MoveGroupInterfacePtr mgi_;
|
||||
};
|
||||
|
||||
} } }
|
||||
|
||||
@ -13,44 +13,51 @@
|
||||
namespace moveit { namespace task_constructor { namespace stages {
|
||||
|
||||
CartesianPositionMotion::CartesianPositionMotion(std::string name)
|
||||
: PropagatingEitherWay(name),
|
||||
step_size_(0.005)
|
||||
: PropagatingEitherWay(name)
|
||||
{
|
||||
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){
|
||||
group_= group;
|
||||
setProperty("group", group);
|
||||
}
|
||||
|
||||
void CartesianPositionMotion::setLink(std::string link){
|
||||
link_= link;
|
||||
setProperty("link", link);
|
||||
}
|
||||
|
||||
void CartesianPositionMotion::setMinDistance(double distance){
|
||||
min_distance_= distance;
|
||||
setProperty("min_distance", distance);
|
||||
}
|
||||
|
||||
void CartesianPositionMotion::setMaxDistance(double distance){
|
||||
max_distance_= distance;
|
||||
setProperty("max_distance", distance);
|
||||
}
|
||||
|
||||
void CartesianPositionMotion::setMinMaxDistance(double min_distance, double max_distance){
|
||||
setMinDistance(min_distance);
|
||||
setMaxDistance(max_distance);
|
||||
setProperty("min_distance", min_distance);
|
||||
setProperty("max_distance", max_distance);
|
||||
}
|
||||
|
||||
void CartesianPositionMotion::towards(geometry_msgs::PointStamped towards){
|
||||
mode_= CartesianPositionMotion::MODE_TOWARDS;
|
||||
towards_= towards;
|
||||
setProperty("towards", towards);
|
||||
}
|
||||
|
||||
void CartesianPositionMotion::along(geometry_msgs::Vector3Stamped along){
|
||||
mode_= CartesianPositionMotion::MODE_ALONG;
|
||||
along_= along;
|
||||
setProperty("along", along);
|
||||
}
|
||||
|
||||
void CartesianPositionMotion::setCartesianStepSize(double distance){
|
||||
step_size_= distance;
|
||||
setProperty("step_size", distance);
|
||||
}
|
||||
|
||||
namespace {
|
||||
@ -68,8 +75,12 @@ bool CartesianPositionMotion::computeForward(const InterfaceState &from){
|
||||
planning_scene::PlanningScenePtr result_scene = from.scene()->diff();
|
||||
robot_state::RobotState &robot_state = result_scene->getCurrentStateNonConst();
|
||||
|
||||
const moveit::core::JointModelGroup* jmg= robot_state.getJointModelGroup(group_);
|
||||
const moveit::core::LinkModel* link_model= robot_state.getRobotModel()->getLinkModel(link_);
|
||||
const auto& props = properties();
|
||||
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=
|
||||
std::bind(
|
||||
@ -83,12 +94,12 @@ bool CartesianPositionMotion::computeForward(const InterfaceState &from){
|
||||
bool succeeded= false;
|
||||
|
||||
if( mode_ == CartesianPositionMotion::MODE_TOWARDS ){
|
||||
const Eigen::Affine3d& frame= from.scene()->getFrameTransform(towards_.header.frame_id);
|
||||
|
||||
const Eigen::Affine3d& link_pose= robot_state.getGlobalLinkTransform(link_);
|
||||
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);
|
||||
|
||||
Eigen::Vector3d target_point;
|
||||
tf::pointMsgToEigen(towards_.point, target_point);
|
||||
tf::pointMsgToEigen(towards.point, target_point);
|
||||
target_point= frame*target_point;
|
||||
|
||||
// retain orientation of link
|
||||
@ -101,7 +112,7 @@ bool CartesianPositionMotion::computeForward(const InterfaceState &from){
|
||||
link_model,
|
||||
target,
|
||||
true, /* global frame */
|
||||
step_size_, /* cartesian step size */
|
||||
props.get<double>("step_size"), /* cartesian step size */
|
||||
1.5, /* jump threshold */
|
||||
is_valid);
|
||||
|
||||
@ -109,12 +120,13 @@ bool CartesianPositionMotion::computeForward(const InterfaceState &from){
|
||||
|
||||
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 ){
|
||||
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;
|
||||
tf::vectorMsgToEigen(along_.vector, direction);
|
||||
tf::vectorMsgToEigen(along.vector, direction);
|
||||
direction= frame.linear()*direction;
|
||||
|
||||
double achieved_distance= robot_state.computeCartesianPath(
|
||||
@ -123,14 +135,14 @@ bool CartesianPositionMotion::computeForward(const InterfaceState &from){
|
||||
link_model,
|
||||
direction,
|
||||
true, /* global frame */
|
||||
max_distance_, /* distance */
|
||||
step_size_, /* cartesian step size */
|
||||
props.get<double>("max_distance"),
|
||||
props.get<double>("step_size"), /* cartesian step size */
|
||||
1.5, /* jump threshold */
|
||||
is_valid);
|
||||
|
||||
std::cout << "achieved " << achieved_distance << " of cartesian motion" << std::endl;
|
||||
|
||||
succeeded= achieved_distance >= min_distance_;
|
||||
succeeded= achieved_distance >= props.get<double>("min_distance");
|
||||
}
|
||||
else
|
||||
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();
|
||||
robot_state::RobotState &robot_state = result_scene->getCurrentStateNonConst();
|
||||
|
||||
const moveit::core::JointModelGroup* jmg= robot_state.getJointModelGroup(group_);
|
||||
const moveit::core::LinkModel* link_model= robot_state.getRobotModel()->getLinkModel(link_);
|
||||
const auto& props = properties();
|
||||
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=
|
||||
std::bind(
|
||||
@ -167,14 +183,15 @@ bool CartesianPositionMotion::computeBackward(const InterfaceState &to){
|
||||
switch(mode_){
|
||||
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);
|
||||
}
|
||||
break;
|
||||
case(CartesianPositionMotion::MODE_ALONG):
|
||||
{
|
||||
const Eigen::Affine3d& frame= robot_state.getGlobalLinkTransform(along_.header.frame_id);
|
||||
tf::vectorMsgToEigen(along_.vector, direction);
|
||||
const geometry_msgs::Vector3Stamped& along = props.get<geometry_msgs::Vector3Stamped>("along");
|
||||
const Eigen::Affine3d& frame= robot_state.getGlobalLinkTransform(along.header.frame_id);
|
||||
tf::vectorMsgToEigen(along.vector, direction);
|
||||
direction= frame.linear()*direction;
|
||||
}
|
||||
break;
|
||||
@ -190,14 +207,14 @@ bool CartesianPositionMotion::computeBackward(const InterfaceState &to){
|
||||
link_model,
|
||||
direction,
|
||||
true, /* global frame */
|
||||
max_distance_, /* distance */
|
||||
step_size_, /* cartesian step size */
|
||||
props.get<double>("max_distance"),
|
||||
props.get<double>("step_size"), /* cartesian step size */
|
||||
1.5, /* jump threshold */
|
||||
is_valid);
|
||||
|
||||
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){
|
||||
robot_trajectory::RobotTrajectoryPtr traj= std::make_shared<robot_trajectory::RobotTrajectory>(robot_state.getRobotModel(), jmg);
|
||||
|
||||
@ -21,6 +21,17 @@ namespace moveit { namespace task_constructor { namespace stages {
|
||||
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>("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)
|
||||
@ -30,40 +41,44 @@ void GenerateGraspPose::init(const planning_scene::PlanningSceneConstPtr &scene)
|
||||
}
|
||||
|
||||
void GenerateGraspPose::setGroup(std::string group){
|
||||
group_= group;
|
||||
setProperty("group", group);
|
||||
}
|
||||
|
||||
void GenerateGraspPose::setLink(std::string ik_link){
|
||||
ik_link_= ik_link;
|
||||
setProperty("link", ik_link);
|
||||
}
|
||||
|
||||
void GenerateGraspPose::setEndEffector(std::string eef){
|
||||
eef_= eef;
|
||||
setProperty("eef", eef);
|
||||
}
|
||||
|
||||
void GenerateGraspPose::setGripperGraspPose(std::string pose_name){
|
||||
gripper_grasp_pose_= pose_name;
|
||||
setProperty("grasp_pose", pose_name);
|
||||
}
|
||||
|
||||
void GenerateGraspPose::setObject(std::string object){
|
||||
object_= object;
|
||||
setProperty("object", object);
|
||||
}
|
||||
|
||||
void GenerateGraspPose::setGraspOffset(double offset){
|
||||
grasp_offset_= offset;
|
||||
setProperty("grasp_offset", offset);
|
||||
}
|
||||
|
||||
void GenerateGraspPose::setTimeout(double timeout){
|
||||
timeout_= timeout;
|
||||
remaining_time_= timeout;
|
||||
setProperty("timeout", timeout);
|
||||
}
|
||||
|
||||
void GenerateGraspPose::setAngleDelta(double delta){
|
||||
angle_delta_= delta;
|
||||
setProperty("angle_delta", delta);
|
||||
}
|
||||
|
||||
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{
|
||||
@ -98,48 +113,58 @@ namespace {
|
||||
}
|
||||
|
||||
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();
|
||||
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(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()){
|
||||
grasp_state.setToDefaultValues(jmg_eef, gripper_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);
|
||||
}
|
||||
|
||||
const moveit::core::GroupStateValidityCallbackFn is_valid=
|
||||
std::bind(
|
||||
&isValid,
|
||||
scene_,
|
||||
ignore_collisions_,
|
||||
props.get<bool>("ignore_collisions"),
|
||||
&previous_solutions_,
|
||||
std::placeholders::_1,
|
||||
std::placeholders::_2,
|
||||
std::placeholders::_3);
|
||||
|
||||
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())
|
||||
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_)){
|
||||
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_+= angle_delta_;
|
||||
remaining_time_= timeout_;
|
||||
<< " 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;
|
||||
@ -147,8 +172,8 @@ bool GenerateGraspPose::compute(){
|
||||
|
||||
grasp_pose= object_pose;
|
||||
|
||||
grasp_pose.position.x-= grasp_offset_*cos(current_angle_);
|
||||
grasp_pose.position.y-= grasp_offset_*sin(current_angle_);
|
||||
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_)
|
||||
@ -156,8 +181,8 @@ bool GenerateGraspPose::compute(){
|
||||
tried_current_state_as_seed_= true;
|
||||
|
||||
auto now= std::chrono::steady_clock::now();
|
||||
bool succeeded= grasp_state.setFromIK(jmg_active, grasp_pose, ik_link, 1, remaining_time_, is_valid);
|
||||
remaining_time_-= std::chrono::duration<double>(std::chrono::steady_clock::now()- now).count();
|
||||
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();
|
||||
|
||||
if(succeeded) {
|
||||
previous_solutions_.emplace_back();
|
||||
|
||||
@ -15,7 +15,13 @@ namespace moveit { namespace task_constructor { namespace stages {
|
||||
|
||||
Gripper::Gripper(std::string 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)
|
||||
{
|
||||
@ -24,25 +30,25 @@ void Gripper::init(const planning_scene::PlanningSceneConstPtr &scene)
|
||||
}
|
||||
|
||||
void Gripper::setEndEffector(std::string eef){
|
||||
eef_= eef;
|
||||
setProperty("eef", eef);
|
||||
}
|
||||
|
||||
void Gripper::setAttachLink(std::string link){
|
||||
attach_link_= link;
|
||||
setProperty("link", link);
|
||||
}
|
||||
|
||||
void Gripper::setFrom(std::string named_target){
|
||||
restrictDirection(BACKWARD);
|
||||
named_target_= named_target;
|
||||
setProperty("named_target", named_target);
|
||||
}
|
||||
|
||||
void Gripper::setTo(std::string named_target){
|
||||
restrictDirection(FORWARD);
|
||||
named_target_= named_target;
|
||||
setProperty("named_target", named_target);
|
||||
}
|
||||
|
||||
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,
|
||||
@ -50,24 +56,30 @@ bool Gripper::compute(const InterfaceState &state, planning_scene::PlanningScene
|
||||
scene = state.scene()->diff();
|
||||
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_){
|
||||
assert(scene->getRobotModel()->hasEndEffector(eef_) && "no end effector with that name defined in srdf");
|
||||
const moveit::core::JointModelGroup* jmg= scene->getRobotModel()->getEndEffector(eef_);
|
||||
assert(scene->getRobotModel()->hasEndEffector(eef) && "no end effector with that name defined in srdf");
|
||||
const moveit::core::JointModelGroup* jmg= scene->getRobotModel()->getEndEffector(eef);
|
||||
const std::string group_name= jmg->getName();
|
||||
mgi_= std::make_shared<moveit::planning_interface::MoveGroupInterface>(group_name);
|
||||
|
||||
if( attach_link_.empty() ){
|
||||
attach_link_= jmg->getEndEffectorParentGroup().second;
|
||||
if( link.empty() ){
|
||||
link= jmg->getEndEffectorParentGroup().second;
|
||||
}
|
||||
}
|
||||
|
||||
mgi_->setNamedTarget(named_target_);
|
||||
mgi_->setNamedTarget(named_target);
|
||||
|
||||
::planning_interface::MotionPlanRequest req;
|
||||
mgi_->constructMotionPlanRequest(req);
|
||||
|
||||
if( !grasp_object_.empty() ){
|
||||
scene->getAllowedCollisionMatrixNonConst().setEntry(grasp_object_, mgi_->getLinkNames(), true);
|
||||
if( !grasp_object.empty() ){
|
||||
scene->getAllowedCollisionMatrixNonConst().setEntry(grasp_object, mgi_->getLinkNames(), true);
|
||||
}
|
||||
|
||||
::planning_interface::MotionPlanResponse res;
|
||||
@ -80,10 +92,10 @@ bool Gripper::compute(const InterfaceState &state, planning_scene::PlanningScene
|
||||
scene->setCurrentState(trajectory->getLastWayPoint());
|
||||
|
||||
// attach object
|
||||
if( !grasp_object_.empty() ){
|
||||
if( !grasp_object.empty() ){
|
||||
moveit_msgs::AttachedCollisionObject obj;
|
||||
obj.link_name= attach_link_;
|
||||
obj.object.id= grasp_object_;
|
||||
obj.link_name= link;
|
||||
obj.object.id= grasp_object;
|
||||
scene->processAttachedCollisionObjectMsg(obj);
|
||||
}
|
||||
|
||||
|
||||
@ -14,9 +14,13 @@
|
||||
namespace moveit { namespace task_constructor { namespace stages {
|
||||
|
||||
Move::Move(std::string name)
|
||||
: Connecting(name),
|
||||
timeout_(5.0)
|
||||
{}
|
||||
: Connecting(name)
|
||||
{
|
||||
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)
|
||||
{
|
||||
@ -24,27 +28,30 @@ void Move::init(const planning_scene::PlanningSceneConstPtr &scene)
|
||||
planner_ = Task::createPlanner(scene->getRobotModel());
|
||||
}
|
||||
|
||||
void Move::setGroup(std::string group){
|
||||
group_= group;
|
||||
mgi_= std::make_shared<moveit::planning_interface::MoveGroupInterface>(group_);
|
||||
void Move::setGroup(const std::string& group){
|
||||
setProperty("group", group);
|
||||
}
|
||||
|
||||
void Move::setPlannerId(std::string planner){
|
||||
planner_id_= planner;
|
||||
void Move::setPlannerId(const std::string& planner){
|
||||
setProperty("planner", planner);
|
||||
}
|
||||
|
||||
void Move::setTimeout(double timeout){
|
||||
timeout_= timeout;
|
||||
setProperty("timeout", timeout);
|
||||
}
|
||||
|
||||
bool Move::compute(const InterfaceState &from, const InterfaceState &to) {
|
||||
mgi_->setJointValueTarget(to.scene()->getCurrentState());
|
||||
if( !planner_id_.empty() )
|
||||
mgi_->setPlannerId(planner_id_);
|
||||
mgi_->setPlanningTime(timeout_);
|
||||
const auto& props = properties();
|
||||
moveit::planning_interface::MoveGroupInterface mgi(props.get<std::string>("group"));
|
||||
mgi.setJointValueTarget(to.scene()->getCurrentState());
|
||||
|
||||
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;
|
||||
mgi_->constructMotionPlanRequest(req);
|
||||
mgi.constructMotionPlanRequest(req);
|
||||
|
||||
ros::Duration(4.0).sleep();
|
||||
::planning_interface::MotionPlanResponse res;
|
||||
|
||||
Loading…
Reference in New Issue
Block a user