mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
use namespace definitions
This commit is contained in:
parent
bb1fcad44e
commit
f7fe7fc896
@ -5,7 +5,7 @@
|
|||||||
#include <moveit_task_constructor/subtask.h>
|
#include <moveit_task_constructor/subtask.h>
|
||||||
|
|
||||||
namespace moveit { namespace planning_interface {
|
namespace moveit { namespace planning_interface {
|
||||||
MOVEIT_CLASS_FORWARD(MoveGroupInterface);
|
MOVEIT_CLASS_FORWARD(MoveGroupInterface)
|
||||||
} }
|
} }
|
||||||
|
|
||||||
namespace moveit { namespace task_constructor { namespace subtasks {
|
namespace moveit { namespace task_constructor { namespace subtasks {
|
||||||
|
|||||||
@ -12,8 +12,10 @@
|
|||||||
|
|
||||||
#include <Eigen/Geometry>
|
#include <Eigen/Geometry>
|
||||||
|
|
||||||
moveit::task_constructor::subtasks::CartesianPositionMotion::CartesianPositionMotion(std::string name)
|
namespace moveit { namespace task_constructor { namespace subtasks {
|
||||||
: moveit::task_constructor::SubTask::SubTask(name),
|
|
||||||
|
CartesianPositionMotion::CartesianPositionMotion(std::string name)
|
||||||
|
: SubTask(name),
|
||||||
step_size_(0.005)
|
step_size_(0.005)
|
||||||
{
|
{
|
||||||
ros::NodeHandle nh;
|
ros::NodeHandle nh;
|
||||||
@ -21,51 +23,42 @@ moveit::task_constructor::subtasks::CartesianPositionMotion::CartesianPositionMo
|
|||||||
ros::Duration(1.0).sleep();
|
ros::Duration(1.0).sleep();
|
||||||
}
|
}
|
||||||
|
|
||||||
void
|
void CartesianPositionMotion::setGroup(std::string group){
|
||||||
moveit::task_constructor::subtasks::CartesianPositionMotion::setGroup(std::string group){
|
|
||||||
group_= group;
|
group_= group;
|
||||||
}
|
}
|
||||||
|
|
||||||
void
|
void CartesianPositionMotion::setLink(std::string link){
|
||||||
moveit::task_constructor::subtasks::CartesianPositionMotion::setLink(std::string link){
|
|
||||||
link_= link;
|
link_= link;
|
||||||
}
|
}
|
||||||
|
|
||||||
void
|
void CartesianPositionMotion::setMinDistance(double distance){
|
||||||
moveit::task_constructor::subtasks::CartesianPositionMotion::setMinDistance(double distance){
|
|
||||||
min_distance_= distance;
|
min_distance_= distance;
|
||||||
}
|
}
|
||||||
|
|
||||||
void
|
void CartesianPositionMotion::setMaxDistance(double distance){
|
||||||
moveit::task_constructor::subtasks::CartesianPositionMotion::setMaxDistance(double distance){
|
|
||||||
max_distance_= distance;
|
max_distance_= distance;
|
||||||
}
|
}
|
||||||
|
|
||||||
void
|
void CartesianPositionMotion::setMinMaxDistance(double min_distance, double max_distance){
|
||||||
moveit::task_constructor::subtasks::CartesianPositionMotion::setMinMaxDistance(double min_distance, double max_distance){
|
|
||||||
setMinDistance(min_distance);
|
setMinDistance(min_distance);
|
||||||
setMaxDistance(max_distance);
|
setMaxDistance(max_distance);
|
||||||
}
|
}
|
||||||
|
|
||||||
void
|
void CartesianPositionMotion::towards(geometry_msgs::PointStamped towards){
|
||||||
moveit::task_constructor::subtasks::CartesianPositionMotion::towards(geometry_msgs::PointStamped towards){
|
mode_= CartesianPositionMotion::MODE_TOWARDS;
|
||||||
mode_= moveit::task_constructor::subtasks::CartesianPositionMotion::MODE_TOWARDS;
|
|
||||||
towards_= towards;
|
towards_= towards;
|
||||||
}
|
}
|
||||||
|
|
||||||
void
|
void CartesianPositionMotion::along(geometry_msgs::Vector3Stamped along){
|
||||||
moveit::task_constructor::subtasks::CartesianPositionMotion::along(geometry_msgs::Vector3Stamped along){
|
mode_= CartesianPositionMotion::MODE_ALONG;
|
||||||
mode_= moveit::task_constructor::subtasks::CartesianPositionMotion::MODE_ALONG;
|
|
||||||
along_= along;
|
along_= along;
|
||||||
}
|
}
|
||||||
|
|
||||||
void
|
void CartesianPositionMotion::setCartesianStepSize(double distance){
|
||||||
moveit::task_constructor::subtasks::CartesianPositionMotion::setCartesianStepSize(double distance){
|
|
||||||
step_size_= distance;
|
step_size_= distance;
|
||||||
}
|
}
|
||||||
|
|
||||||
bool
|
bool CartesianPositionMotion::canCompute(){
|
||||||
moveit::task_constructor::subtasks::CartesianPositionMotion::canCompute(){
|
|
||||||
return hasBeginning() || hasEnding();
|
return hasBeginning() || hasEnding();
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -80,16 +73,14 @@ namespace {
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
bool
|
bool CartesianPositionMotion::compute(){
|
||||||
moveit::task_constructor::subtasks::CartesianPositionMotion::compute(){
|
|
||||||
if( hasEnding() )
|
if( hasEnding() )
|
||||||
return _computeFromEnding();
|
return _computeFromEnding();
|
||||||
else if( hasBeginning() )
|
else if( hasBeginning() )
|
||||||
return _computeFromBeginning();
|
return _computeFromBeginning();
|
||||||
}
|
}
|
||||||
|
|
||||||
bool
|
bool CartesianPositionMotion::_computeFromBeginning(){
|
||||||
moveit::task_constructor::subtasks::CartesianPositionMotion::_computeFromBeginning(){
|
|
||||||
assert( hasBeginning() );
|
assert( hasBeginning() );
|
||||||
|
|
||||||
moveit::task_constructor::InterfaceState& beginning= fetchStateBeginning();
|
moveit::task_constructor::InterfaceState& beginning= fetchStateBeginning();
|
||||||
@ -110,7 +101,7 @@ moveit::task_constructor::subtasks::CartesianPositionMotion::_computeFromBeginni
|
|||||||
std::vector<moveit::core::RobotStatePtr> trajectory_steps;
|
std::vector<moveit::core::RobotStatePtr> trajectory_steps;
|
||||||
bool succeeded= false;
|
bool succeeded= false;
|
||||||
|
|
||||||
if( mode_ == moveit::task_constructor::subtasks::CartesianPositionMotion::MODE_TOWARDS ){
|
if( mode_ == CartesianPositionMotion::MODE_TOWARDS ){
|
||||||
const Eigen::Affine3d& frame= beginning.state->getFrameTransform(towards_.header.frame_id);
|
const Eigen::Affine3d& frame= beginning.state->getFrameTransform(towards_.header.frame_id);
|
||||||
|
|
||||||
const Eigen::Affine3d& link_pose= robot_state.getGlobalLinkTransform(link_);
|
const Eigen::Affine3d& link_pose= robot_state.getGlobalLinkTransform(link_);
|
||||||
@ -139,7 +130,7 @@ moveit::task_constructor::subtasks::CartesianPositionMotion::_computeFromBeginni
|
|||||||
|
|
||||||
succeeded= achieved_distance >= min_distance_;
|
succeeded= achieved_distance >= min_distance_;
|
||||||
}
|
}
|
||||||
else if( mode_ == moveit::task_constructor::subtasks::CartesianPositionMotion::MODE_ALONG ){
|
else if( mode_ == CartesianPositionMotion::MODE_ALONG ){
|
||||||
const Eigen::Affine3d& frame= robot_state.getGlobalLinkTransform(along_.header.frame_id);
|
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);
|
||||||
@ -184,8 +175,7 @@ moveit::task_constructor::subtasks::CartesianPositionMotion::_computeFromBeginni
|
|||||||
return succeeded;
|
return succeeded;
|
||||||
}
|
}
|
||||||
|
|
||||||
bool
|
bool CartesianPositionMotion::_computeFromEnding(){
|
||||||
moveit::task_constructor::subtasks::CartesianPositionMotion::_computeFromEnding(){
|
|
||||||
assert( hasEnding() );
|
assert( hasEnding() );
|
||||||
|
|
||||||
moveit::task_constructor::InterfaceState& ending= fetchStateEnding();
|
moveit::task_constructor::InterfaceState& ending= fetchStateEnding();
|
||||||
@ -206,13 +196,13 @@ moveit::task_constructor::subtasks::CartesianPositionMotion::_computeFromEnding(
|
|||||||
Eigen::Vector3d direction;
|
Eigen::Vector3d direction;
|
||||||
|
|
||||||
switch(mode_){
|
switch(mode_){
|
||||||
case(moveit::task_constructor::subtasks::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(moveit::task_constructor::subtasks::CartesianPositionMotion::MODE_ALONG):
|
case(CartesianPositionMotion::MODE_ALONG):
|
||||||
{
|
{
|
||||||
const Eigen::Affine3d& frame= robot_state.getGlobalLinkTransform(along_.header.frame_id);
|
const Eigen::Affine3d& frame= robot_state.getGlobalLinkTransform(along_.header.frame_id);
|
||||||
tf::vectorMsgToEigen(along_.vector, direction);
|
tf::vectorMsgToEigen(along_.vector, direction);
|
||||||
@ -263,8 +253,7 @@ moveit::task_constructor::subtasks::CartesianPositionMotion::_computeFromEnding(
|
|||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
void
|
void CartesianPositionMotion::_publishTrajectory(const robot_trajectory::RobotTrajectory& trajectory, const moveit::core::RobotState& start){
|
||||||
moveit::task_constructor::subtasks::CartesianPositionMotion::_publishTrajectory(const robot_trajectory::RobotTrajectory& trajectory, const moveit::core::RobotState& start){
|
|
||||||
moveit_msgs::DisplayTrajectory dt;
|
moveit_msgs::DisplayTrajectory dt;
|
||||||
robot_state::robotStateToRobotStateMsg(start, dt.trajectory_start);
|
robot_state::robotStateToRobotStateMsg(start, dt.trajectory_start);
|
||||||
dt.model_id= scene_->getRobotModel()->getName();
|
dt.model_id= scene_->getRobotModel()->getName();
|
||||||
@ -273,3 +262,5 @@ moveit::task_constructor::subtasks::CartesianPositionMotion::_publishTrajectory(
|
|||||||
dt.model_id= start.getRobotModel()->getName();
|
dt.model_id= start.getRobotModel()->getName();
|
||||||
pub.publish(dt);
|
pub.publish(dt);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
} } }
|
||||||
|
|||||||
@ -1,18 +1,18 @@
|
|||||||
#include <moveit_task_constructor/subtasks/current_state.h>
|
#include <moveit_task_constructor/subtasks/current_state.h>
|
||||||
|
|
||||||
moveit::task_constructor::subtasks::CurrentState::CurrentState(std::string name)
|
namespace moveit { namespace task_constructor { namespace subtasks {
|
||||||
: moveit::task_constructor::SubTask::SubTask(name)
|
|
||||||
|
CurrentState::CurrentState(std::string name)
|
||||||
|
: SubTask(name)
|
||||||
{
|
{
|
||||||
ran_= false;
|
ran_= false;
|
||||||
}
|
}
|
||||||
|
|
||||||
bool
|
bool CurrentState::canCompute(){
|
||||||
moveit::task_constructor::subtasks::CurrentState::canCompute(){
|
|
||||||
return !ran_;
|
return !ran_;
|
||||||
}
|
}
|
||||||
|
|
||||||
bool
|
bool CurrentState::compute(){
|
||||||
moveit::task_constructor::subtasks::CurrentState::compute(){
|
|
||||||
ran_= true;
|
ran_= true;
|
||||||
|
|
||||||
assert( scene_ );
|
assert( scene_ );
|
||||||
@ -25,3 +25,5 @@ moveit::task_constructor::subtasks::CurrentState::compute(){
|
|||||||
|
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
} } }
|
||||||
|
|||||||
@ -15,8 +15,10 @@
|
|||||||
#include <chrono>
|
#include <chrono>
|
||||||
#include <functional>
|
#include <functional>
|
||||||
|
|
||||||
moveit::task_constructor::subtasks::GenerateGraspPose::GenerateGraspPose(std::string name)
|
namespace moveit { namespace task_constructor { namespace subtasks {
|
||||||
: moveit::task_constructor::SubTask::SubTask(name),
|
|
||||||
|
GenerateGraspPose::GenerateGraspPose(std::string name)
|
||||||
|
: SubTask(name),
|
||||||
timeout_(0.1),
|
timeout_(0.1),
|
||||||
angle_delta_(0.1),
|
angle_delta_(0.1),
|
||||||
max_ik_solutions_(0),
|
max_ik_solutions_(0),
|
||||||
@ -31,54 +33,44 @@ moveit::task_constructor::subtasks::GenerateGraspPose::GenerateGraspPose(std::st
|
|||||||
ros::Duration(1.0).sleep();
|
ros::Duration(1.0).sleep();
|
||||||
}
|
}
|
||||||
|
|
||||||
void
|
void GenerateGraspPose::setGroup(std::string group){
|
||||||
moveit::task_constructor::subtasks::GenerateGraspPose::setGroup(std::string group){
|
|
||||||
group_= group;
|
group_= group;
|
||||||
}
|
}
|
||||||
|
|
||||||
void
|
void GenerateGraspPose::setLink(std::string ik_link){
|
||||||
moveit::task_constructor::subtasks::GenerateGraspPose::setLink(std::string ik_link){
|
|
||||||
ik_link_= ik_link;
|
ik_link_= ik_link;
|
||||||
}
|
}
|
||||||
|
|
||||||
void
|
void GenerateGraspPose::setEndEffector(std::string eef){
|
||||||
moveit::task_constructor::subtasks::GenerateGraspPose::setEndEffector(std::string eef){
|
|
||||||
eef_= eef;
|
eef_= eef;
|
||||||
}
|
}
|
||||||
|
|
||||||
void
|
void GenerateGraspPose::setGripperGraspPose(std::string pose_name){
|
||||||
moveit::task_constructor::subtasks::GenerateGraspPose::setGripperGraspPose(std::string pose_name){
|
|
||||||
gripper_grasp_pose_= pose_name;
|
gripper_grasp_pose_= pose_name;
|
||||||
}
|
}
|
||||||
|
|
||||||
void
|
void GenerateGraspPose::setObject(std::string object){
|
||||||
moveit::task_constructor::subtasks::GenerateGraspPose::setObject(std::string object){
|
|
||||||
object_= object;
|
object_= object;
|
||||||
}
|
}
|
||||||
|
|
||||||
void
|
void GenerateGraspPose::setGraspOffset(double offset){
|
||||||
moveit::task_constructor::subtasks::GenerateGraspPose::setGraspOffset(double offset){
|
|
||||||
grasp_offset_= offset;
|
grasp_offset_= offset;
|
||||||
}
|
}
|
||||||
|
|
||||||
void
|
void GenerateGraspPose::setTimeout(double timeout){
|
||||||
moveit::task_constructor::subtasks::GenerateGraspPose::setTimeout(double timeout){
|
|
||||||
timeout_= timeout;
|
timeout_= timeout;
|
||||||
remaining_time_= timeout;
|
remaining_time_= timeout;
|
||||||
}
|
}
|
||||||
|
|
||||||
void
|
void GenerateGraspPose::setAngleDelta(double delta){
|
||||||
moveit::task_constructor::subtasks::GenerateGraspPose::setAngleDelta(double delta){
|
|
||||||
angle_delta_= delta;
|
angle_delta_= delta;
|
||||||
}
|
}
|
||||||
|
|
||||||
void
|
void GenerateGraspPose::setMaxIKSolutions(uint32_t n){
|
||||||
moveit::task_constructor::subtasks::GenerateGraspPose::setMaxIKSolutions(uint32_t n){
|
|
||||||
max_ik_solutions_= n;
|
max_ik_solutions_= n;
|
||||||
}
|
}
|
||||||
|
|
||||||
bool
|
bool GenerateGraspPose::canCompute(){
|
||||||
moveit::task_constructor::subtasks::GenerateGraspPose::canCompute(){
|
|
||||||
return current_angle_ < 2*M_PI && current_angle_ > -2*M_PI;
|
return current_angle_ < 2*M_PI && current_angle_ > -2*M_PI;
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -109,8 +101,7 @@ namespace {
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
bool
|
bool GenerateGraspPose::compute(){
|
||||||
moveit::task_constructor::subtasks::GenerateGraspPose::compute(){
|
|
||||||
assert( scene_->getRobotModel()->hasEndEffector(eef_) && "The specified end effector is not defined in the srdf" );
|
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();
|
||||||
@ -190,3 +181,5 @@ moveit::task_constructor::subtasks::GenerateGraspPose::compute(){
|
|||||||
|
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
} } }
|
||||||
|
|||||||
@ -9,42 +9,37 @@
|
|||||||
#include <moveit_msgs/MotionPlanRequest.h>
|
#include <moveit_msgs/MotionPlanRequest.h>
|
||||||
#include <moveit_msgs/MotionPlanResponse.h>
|
#include <moveit_msgs/MotionPlanResponse.h>
|
||||||
|
|
||||||
moveit::task_constructor::subtasks::Gripper::Gripper(std::string name)
|
namespace moveit { namespace task_constructor { namespace subtasks {
|
||||||
: moveit::task_constructor::SubTask::SubTask(name)
|
|
||||||
|
Gripper::Gripper(std::string name)
|
||||||
|
: SubTask(name)
|
||||||
{}
|
{}
|
||||||
|
|
||||||
void
|
void Gripper::setEndEffector(std::string eef){
|
||||||
moveit::task_constructor::subtasks::Gripper::setEndEffector(std::string eef){
|
|
||||||
eef_= eef;
|
eef_= eef;
|
||||||
}
|
}
|
||||||
|
|
||||||
void
|
void Gripper::setAttachLink(std::string link){
|
||||||
moveit::task_constructor::subtasks::Gripper::setAttachLink(std::string link){
|
|
||||||
attach_link_= link;
|
attach_link_= link;
|
||||||
}
|
}
|
||||||
|
|
||||||
void
|
void Gripper::setFrom(std::string named_target){
|
||||||
moveit::task_constructor::subtasks::Gripper::setFrom(std::string named_target){
|
|
||||||
from_named_target_= named_target;
|
from_named_target_= named_target;
|
||||||
}
|
}
|
||||||
|
|
||||||
void
|
void Gripper::setTo(std::string named_target){
|
||||||
moveit::task_constructor::subtasks::Gripper::setTo(std::string named_target){
|
|
||||||
to_named_target_= named_target;
|
to_named_target_= named_target;
|
||||||
}
|
}
|
||||||
|
|
||||||
void
|
void Gripper::graspObject(std::string grasp_object){
|
||||||
moveit::task_constructor::subtasks::Gripper::graspObject(std::string grasp_object){
|
|
||||||
grasp_object_= grasp_object;
|
grasp_object_= grasp_object;
|
||||||
}
|
}
|
||||||
|
|
||||||
bool
|
bool Gripper::canCompute(){
|
||||||
moveit::task_constructor::subtasks::Gripper::canCompute(){
|
|
||||||
return hasBeginning();
|
return hasBeginning();
|
||||||
}
|
}
|
||||||
|
|
||||||
bool
|
bool Gripper::compute(){
|
||||||
moveit::task_constructor::subtasks::Gripper::compute(){
|
|
||||||
assert( scene_->getRobotModel() );
|
assert( scene_->getRobotModel() );
|
||||||
|
|
||||||
if(!mgi_){
|
if(!mgi_){
|
||||||
@ -88,9 +83,11 @@ moveit::task_constructor::subtasks::Gripper::compute(){
|
|||||||
}
|
}
|
||||||
|
|
||||||
// finish subtask
|
// finish subtask
|
||||||
moveit::task_constructor::SubTrajectory& trajectory= addTrajectory(res.trajectory_);
|
SubTrajectory& trajectory= addTrajectory(res.trajectory_);
|
||||||
trajectory.hasBeginning(start);
|
trajectory.hasBeginning(start);
|
||||||
sendForward(trajectory, scene);
|
sendForward(trajectory, scene);
|
||||||
|
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
} } }
|
||||||
|
|||||||
@ -9,51 +9,45 @@
|
|||||||
#include <moveit_msgs/MotionPlanRequest.h>
|
#include <moveit_msgs/MotionPlanRequest.h>
|
||||||
#include <moveit_msgs/MotionPlanResponse.h>
|
#include <moveit_msgs/MotionPlanResponse.h>
|
||||||
|
|
||||||
moveit::task_constructor::subtasks::Move::Move(std::string name)
|
namespace moveit { namespace task_constructor { namespace subtasks {
|
||||||
: moveit::task_constructor::SubTask::SubTask(name),
|
|
||||||
timeout_(5.0)
|
Move::Move(std::string name)
|
||||||
|
: SubTask(name),
|
||||||
|
timeout_(5.0)
|
||||||
{}
|
{}
|
||||||
|
|
||||||
void
|
void Move::setGroup(std::string group){
|
||||||
moveit::task_constructor::subtasks::Move::setGroup(std::string group){
|
|
||||||
group_= group;
|
group_= group;
|
||||||
mgi_= std::make_shared<moveit::planning_interface::MoveGroupInterface>(group_);
|
mgi_= std::make_shared<moveit::planning_interface::MoveGroupInterface>(group_);
|
||||||
}
|
}
|
||||||
|
|
||||||
void
|
void Move::setLink(std::string link){
|
||||||
moveit::task_constructor::subtasks::Move::setLink(std::string link){
|
|
||||||
link_= link;
|
link_= link;
|
||||||
}
|
}
|
||||||
|
|
||||||
void
|
void Move::setPlannerId(std::string planner){
|
||||||
moveit::task_constructor::subtasks::Move::setPlannerId(std::string planner){
|
|
||||||
planner_id_= planner;
|
planner_id_= planner;
|
||||||
}
|
}
|
||||||
|
|
||||||
void
|
void Move::setTimeout(double timeout){
|
||||||
moveit::task_constructor::subtasks::Move::setTimeout(double timeout){
|
|
||||||
timeout_= timeout;
|
timeout_= timeout;
|
||||||
}
|
}
|
||||||
|
|
||||||
/* TODO: implement this in compute
|
/* TODO: implement this in compute
|
||||||
void
|
void Move::setFrom(std::string named_target){
|
||||||
moveit::task_constructor::subtasks::Move::setFrom(std::string named_target){
|
|
||||||
from_named_target_= named_target;
|
from_named_target_= named_target;
|
||||||
}
|
}
|
||||||
|
|
||||||
void
|
void Move::setTo(std::string named_target){
|
||||||
moveit::task_constructor::subtasks::Move::setTo(std::string named_target){
|
|
||||||
to_named_target_= named_target;
|
to_named_target_= named_target;
|
||||||
}
|
}
|
||||||
*/
|
*/
|
||||||
|
|
||||||
bool
|
bool Move::canCompute(){
|
||||||
moveit::task_constructor::subtasks::Move::canCompute(){
|
|
||||||
return hasStatePair();
|
return hasStatePair();
|
||||||
}
|
}
|
||||||
|
|
||||||
bool
|
bool Move::compute(){
|
||||||
moveit::task_constructor::subtasks::Move::compute(){
|
|
||||||
assert( scene_->getRobotModel() );
|
assert( scene_->getRobotModel() );
|
||||||
|
|
||||||
std::pair<InterfaceState&, InterfaceState&> state_pair= fetchStatePair();
|
std::pair<InterfaceState&, InterfaceState&> state_pair= fetchStatePair();
|
||||||
@ -78,3 +72,5 @@ moveit::task_constructor::subtasks::Move::compute(){
|
|||||||
|
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
} } }
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user