mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
276 lines
8.3 KiB
C++
276 lines
8.3 KiB
C++
#include <moveit_task_constructor/subtasks/cartesian_position_motion.h>
|
|
|
|
#include <ros/ros.h>
|
|
#include <moveit_msgs/DisplayTrajectory.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>
|
|
|
|
moveit::task_constructor::subtasks::CartesianPositionMotion::CartesianPositionMotion(std::string name)
|
|
: moveit::task_constructor::SubTask::SubTask(name),
|
|
step_size_(0.005)
|
|
{
|
|
ros::NodeHandle nh;
|
|
pub= nh.advertise<moveit_msgs::DisplayTrajectory>("move_group/display_planned_path", 50);
|
|
ros::Duration(1.0).sleep();
|
|
}
|
|
|
|
void
|
|
moveit::task_constructor::subtasks::CartesianPositionMotion::setGroup(std::string group){
|
|
group_= group;
|
|
}
|
|
|
|
void
|
|
moveit::task_constructor::subtasks::CartesianPositionMotion::setLink(std::string link){
|
|
link_= link;
|
|
}
|
|
|
|
void
|
|
moveit::task_constructor::subtasks::CartesianPositionMotion::setMinDistance(double distance){
|
|
min_distance_= distance;
|
|
}
|
|
|
|
void
|
|
moveit::task_constructor::subtasks::CartesianPositionMotion::setMaxDistance(double distance){
|
|
max_distance_= distance;
|
|
}
|
|
|
|
void
|
|
moveit::task_constructor::subtasks::CartesianPositionMotion::setMinMaxDistance(double min_distance, double max_distance){
|
|
setMinDistance(min_distance);
|
|
setMaxDistance(max_distance);
|
|
}
|
|
|
|
void
|
|
moveit::task_constructor::subtasks::CartesianPositionMotion::towards(geometry_msgs::PointStamped towards){
|
|
mode_= moveit::task_constructor::subtasks::CartesianPositionMotion::MODE_TOWARDS;
|
|
towards_= towards;
|
|
}
|
|
|
|
void
|
|
moveit::task_constructor::subtasks::CartesianPositionMotion::along(geometry_msgs::Vector3Stamped along){
|
|
mode_= moveit::task_constructor::subtasks::CartesianPositionMotion::MODE_ALONG;
|
|
along_= along;
|
|
}
|
|
|
|
void
|
|
moveit::task_constructor::subtasks::CartesianPositionMotion::setCartesianStepSize(double distance){
|
|
step_size_= distance;
|
|
}
|
|
|
|
bool
|
|
moveit::task_constructor::subtasks::CartesianPositionMotion::canCompute(){
|
|
return hasBeginning() || hasEnding();
|
|
}
|
|
|
|
namespace {
|
|
bool isValid(planning_scene::PlanningSceneConstPtr scene,
|
|
robot_state::RobotState* state,
|
|
const robot_model::JointModelGroup* jmg,
|
|
const double* joint_positions){
|
|
state->setJointGroupPositions(jmg, joint_positions);
|
|
state->update();
|
|
return !scene->isStateColliding(*state, jmg->getName());
|
|
}
|
|
}
|
|
|
|
bool
|
|
moveit::task_constructor::subtasks::CartesianPositionMotion::compute(){
|
|
if( hasEnding() )
|
|
return _computeFromEnding();
|
|
else if( hasBeginning() )
|
|
return _computeFromBeginning();
|
|
}
|
|
|
|
bool
|
|
moveit::task_constructor::subtasks::CartesianPositionMotion::_computeFromBeginning(){
|
|
assert( hasBeginning() );
|
|
|
|
moveit::task_constructor::InterfaceState& beginning= fetchStateBeginning();
|
|
planning_scene::PlanningScenePtr result_scene = beginning.state->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 moveit::core::GroupStateValidityCallbackFn is_valid=
|
|
std::bind(
|
|
&isValid,
|
|
result_scene,
|
|
std::placeholders::_1,
|
|
std::placeholders::_2,
|
|
std::placeholders::_3);
|
|
|
|
std::vector<moveit::core::RobotStatePtr> trajectory_steps;
|
|
bool succeeded= false;
|
|
|
|
if( mode_ == moveit::task_constructor::subtasks::CartesianPositionMotion::MODE_TOWARDS ){
|
|
const Eigen::Affine3d& frame= beginning.state->getFrameTransform(towards_.header.frame_id);
|
|
|
|
const Eigen::Affine3d& link_pose= robot_state.getGlobalLinkTransform(link_);
|
|
|
|
Eigen::Vector3d target_point;
|
|
tf::pointMsgToEigen(towards_.point, target_point);
|
|
target_point= frame*target_point;
|
|
|
|
// retain orientation of link
|
|
Eigen::Affine3d target= link_pose;
|
|
target.translation()= target_point;
|
|
|
|
double achieved_fraction= robot_state.computeCartesianPath(
|
|
jmg,
|
|
trajectory_steps,
|
|
link_model,
|
|
target,
|
|
true, /* global frame */
|
|
step_size_, /* cartesian step size */
|
|
1.5, /* jump threshold */
|
|
is_valid);
|
|
|
|
const double achieved_distance= achieved_fraction*(link_pose.translation()-target_point).norm();
|
|
|
|
std::cout << "achieved " << achieved_distance << " of cartesian motion" << std::endl;
|
|
|
|
succeeded= achieved_distance >= min_distance_;
|
|
}
|
|
else if( mode_ == moveit::task_constructor::subtasks::CartesianPositionMotion::MODE_ALONG ){
|
|
const Eigen::Affine3d& frame= robot_state.getGlobalLinkTransform(along_.header.frame_id);
|
|
Eigen::Vector3d direction;
|
|
tf::vectorMsgToEigen(along_.vector, direction);
|
|
direction= frame.linear()*direction;
|
|
|
|
double achieved_distance= robot_state.computeCartesianPath(
|
|
jmg,
|
|
trajectory_steps,
|
|
link_model,
|
|
direction,
|
|
true, /* global frame */
|
|
max_distance_, /* distance */
|
|
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_;
|
|
}
|
|
else
|
|
throw std::runtime_error("position motion has neither a goal nor a direction");
|
|
|
|
|
|
if(succeeded){
|
|
auto traj= std::make_shared<robot_trajectory::RobotTrajectory>(robot_state.getRobotModel(), jmg);
|
|
for( auto& tp : trajectory_steps )
|
|
traj->addSuffixWayPoint(tp, 0.0);
|
|
|
|
moveit::core::RobotStatePtr result_state= trajectory_steps.back();
|
|
robot_state= *result_state;
|
|
|
|
moveit::task_constructor::SubTrajectory &trajectory = addTrajectory(traj);
|
|
|
|
trajectory.hasBeginning(beginning);
|
|
|
|
sendForward(trajectory, result_scene);
|
|
|
|
_publishTrajectory(*traj, *result_state);
|
|
}
|
|
|
|
return succeeded;
|
|
}
|
|
|
|
bool
|
|
moveit::task_constructor::subtasks::CartesianPositionMotion::_computeFromEnding(){
|
|
assert( hasEnding() );
|
|
|
|
moveit::task_constructor::InterfaceState& ending= fetchStateEnding();
|
|
planning_scene::PlanningScenePtr result_scene = ending.state->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 moveit::core::GroupStateValidityCallbackFn is_valid=
|
|
std::bind(
|
|
&isValid,
|
|
result_scene,
|
|
std::placeholders::_1,
|
|
std::placeholders::_2,
|
|
std::placeholders::_3);
|
|
|
|
Eigen::Vector3d direction;
|
|
|
|
switch(mode_){
|
|
case(moveit::task_constructor::subtasks::CartesianPositionMotion::MODE_TOWARDS):
|
|
{
|
|
const Eigen::Affine3d& link_pose= robot_state.getGlobalLinkTransform(link_);
|
|
direction= link_pose.linear()*Eigen::Vector3d(-1,0,0);
|
|
}
|
|
break;
|
|
case(moveit::task_constructor::subtasks::CartesianPositionMotion::MODE_ALONG):
|
|
{
|
|
const Eigen::Affine3d& frame= robot_state.getGlobalLinkTransform(along_.header.frame_id);
|
|
tf::vectorMsgToEigen(along_.vector, direction);
|
|
direction= frame.linear()*direction;
|
|
}
|
|
break;
|
|
default:
|
|
throw std::runtime_error("position motion has neither a goal nor a direction");
|
|
}
|
|
|
|
auto traj= std::make_shared<robot_trajectory::RobotTrajectory>(robot_state.getRobotModel(), jmg);
|
|
std::vector<moveit::core::RobotStatePtr> trajectory_steps;
|
|
|
|
double achieved_distance= robot_state.computeCartesianPath(
|
|
jmg,
|
|
trajectory_steps,
|
|
link_model,
|
|
direction,
|
|
true, /* global frame */
|
|
max_distance_, /* distance */
|
|
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_;
|
|
|
|
if(succeeded){
|
|
for( auto& tp : trajectory_steps )
|
|
traj->addPrefixWayPoint(tp, 0.0);
|
|
|
|
moveit::core::RobotStatePtr result_state= trajectory_steps.back();
|
|
robot_state= *result_state;
|
|
|
|
moveit::task_constructor::SubTrajectory &trajectory = addTrajectory(traj);
|
|
|
|
trajectory.hasEnding(ending);
|
|
|
|
sendBackward(trajectory, result_scene);
|
|
|
|
_publishTrajectory(*traj, *result_state);
|
|
|
|
return true;
|
|
}
|
|
|
|
return false;
|
|
}
|
|
|
|
|
|
void
|
|
moveit::task_constructor::subtasks::CartesianPositionMotion::_publishTrajectory(const robot_trajectory::RobotTrajectory& trajectory, const moveit::core::RobotState& start){
|
|
moveit_msgs::DisplayTrajectory dt;
|
|
robot_state::robotStateToRobotStateMsg(start, dt.trajectory_start);
|
|
dt.model_id= scene_->getRobotModel()->getName();
|
|
dt.trajectory.resize(1);
|
|
trajectory.getRobotTrajectoryMsg(dt.trajectory[0]);
|
|
dt.model_id= start.getRobotModel()->getName();
|
|
pub.publish(dt);
|
|
}
|