#include #include #include #include #include #include #include #include #include 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("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 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_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_state.getRobotModel(), jmg); std::vector 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); }