void MotionPlanningFrame::attachObjectToState(robot_state::RobotState& state,
const std::string& object_id,
const std::string& link_id,
const geometry_msgs::Pose& pose_object_link)
{
state.clearAttachedBodies();
if (object_id.empty() || link_id.empty())
return;
Eigen::Affine3d T_object_link;
tf::poseMsgToEigen(pose_object_link, T_object_link);
attachObjectToState(state, object_id, link_id, T_object_link);
}
void MotionPlanningFrame::attachObjectToState(robot_state::RobotState& state,
const std::string& object_id,
const std::string& link_id,
const Eigen::Affine3d& T_object_link)
{
state.clearAttachedBodies();
if (object_id.empty())
return;
// Compute the object key.
KeyPoseMap world_state = computeWorldKeyPoseMap();
std::string object_key = computeNearestFrameKey(object_id, link_id, state, world_state);
// Extract the object from the world.
collision_detection::CollisionWorld::ObjectConstPtr object;
std::vector<shapes::ShapeConstPtr> shapes;
EigenSTL::vector_Affine3d poses;
{
planning_scene_monitor::LockedPlanningSceneRO ps = planning_display_->getPlanningSceneRO();
object = ps->getWorld()->getObject(object_key);
}
if (!object)
{
if (!object_id.empty())
ROS_WARN("Failed to attach %s to %s. Object does not exist.",
object_id.c_str(), link_id.c_str());
return;
}
// Compute poses relative to link.
shapes = object->shapes_;
poses = object->shape_poses_;
for (int i = 0; i < poses.size(); i++)
poses[i] = T_object_link * poses[0].inverse() * poses[i];
// Attach collision object to robot.
moveit_msgs::AttachedCollisionObject aco; // For dummy arguments.
state.attachBody(object_id, shapes, poses, aco.touch_links, link_id, aco.detach_posture);
// Update state.
state.update();
}
void MotionPlanningFrame::snapStateToFrame(robot_state::RobotState& state,
const Eigen::Affine3d& T_frame_world,
const std::string& link,
const Eigen::Affine3d& T_frame_link,
const std::string& group)
{
ROS_DEBUG_FUNCTION;
// std::cout << "T_frame_world:\n" << T_frame_world.matrix() << std::endl;
// std::cout << "T_frame_link:\n" << T_frame_link.matrix() << std::endl;
// Back out the desired link transform in global coordinates.
Eigen::Affine3d T_link_world = T_frame_world * T_frame_link.inverse();
// Snap to the frame using IK.
const moveit::core::JointModelGroup* jmg = state.getJointModelGroup(group);
// std::cout << "group: " << group << std::endl;
state.update();
trajectory_msgs::JointTrajectoryPoint point;
state.copyJointGroupPositions(jmg, point.positions);
ROS_DEBUG_STREAM("pre-ik positions:\n" << point);
geometry_msgs::Pose pose;
tf::poseEigenToMsg(T_link_world, pose);
state.setFromIK(jmg, pose, link);
state.update();
state.copyJointGroupPositions(jmg, point.positions);
ROS_DEBUG_STREAM("post-ik positions:\n" << point);
ROS_DEBUG_FUNCTION;
}
void MotionPlanningFrame::computeLinearInterpPlan(const robot_state::RobotState& start,
apc_msgs::PrimitiveAction& goal)
{
std::string group = goal.group_name;
const moveit::core::JointModelGroup* joint_group = start.getJointModelGroup(group);
trajectory_msgs::JointTrajectoryPoint s, c, e;
const std::vector<std::string>& joint_names = goal.joint_trajectory.joint_names;
s = goal.joint_trajectory.points.front();
for (int i = 0; i < joint_names.size(); i++)
s.positions[i] = (start.getVariablePosition(joint_names[i]));
e = goal.joint_trajectory.points.back();
goal.joint_trajectory.points.clear();
trajectory_msgs::JointTrajectory& T = goal.joint_trajectory;
const int n = 15;
c = s;
for (int i = 0; i <= n; i++)
{
for (int j=0; j < c.positions.size(); j++)
c.positions[j] = s.positions[j] + i * (e.positions[j] - s.positions[j]) / n;
T.points.push_back(c);
}
}
void ompl_interface::ModelBasedStateSpace::copyJointToOMPLState(ompl::base::State* state,
const robot_state::RobotState& robot_state,
const moveit::core::JointModel* joint_model,
int ompl_state_joint_index) const
{
// Copy one joint (multiple variables possibly)
memcpy(getValueAddressAtIndex(state, ompl_state_joint_index),
robot_state.getVariablePositions() + joint_model->getFirstVariableIndex() * sizeof(double),
joint_model->getVariableCount() * sizeof(double));
// clear any cached info (such as validity known or not)
state->as<StateType>()->clearKnownInformation();
}
// This is intended to be called as a ModifyStateFunction to modify the state
// maintained by a LockedRobotState in place.
bool robot_interaction::KinematicOptions::setStateFromIK(
robot_state::RobotState& state,
const std::string& group,
const std::string& tip,
const geometry_msgs::Pose& pose) const
{
const robot_model::JointModelGroup *jmg = state.getJointModelGroup(group);
if (!jmg)
{
ROS_ERROR("No getJointModelGroup('%s') found",group.c_str());
return false;
}
bool result = state.setFromIK(jmg,
pose,
tip,
max_attempts_,
timeout_seconds_,
state_validity_callback_,
options_);
state.update();
return result;
}
kinematic_constraints::ConstraintEvaluationResult kinematic_constraints::JointConstraint::decide(const robot_state::RobotState &state, bool verbose) const
{
if (!joint_model_)
return ConstraintEvaluationResult(true, 0.0);
const robot_state::JointState *joint = state.getJointState(joint_model_->getName());
if (!joint)
{
logWarn("No joint in state with name '%s'", joint_model_->getName().c_str());
return ConstraintEvaluationResult(true, 0.0);
}
double current_joint_position = joint->getVariableValues()[0];
if (!local_variable_name_.empty())
{
const std::map<std::string, unsigned int> &index_map = joint->getVariableIndexMap();
std::map<std::string, unsigned int>::const_iterator it = index_map.find(joint_variable_name_);
if (it == index_map.end())
{
logWarn("Local name '%s' is not known to joint state with name '%s'", local_variable_name_.c_str(), joint_model_->getName().c_str());
return ConstraintEvaluationResult(true, 0.0);
}
else
current_joint_position = joint->getVariableValues()[it->second];
}
double dif = 0.0;
// compute signed shortest distance for continuous joints
if (joint_is_continuous_)
{
dif = normalizeAngle(current_joint_position) - joint_position_;
if (dif > boost::math::constants::pi<double>())
dif = 2.0*boost::math::constants::pi<double>() - dif;
else
if (dif < -boost::math::constants::pi<double>())
dif += 2.0*boost::math::constants::pi<double>(); // we include a sign change to have dif > 0
}
else
dif = current_joint_position - joint_position_;
// check bounds
bool result = dif <= (joint_tolerance_above_+2*std::numeric_limits<double>::epsilon()) && dif >= (-joint_tolerance_below_-2*std::numeric_limits<double>::epsilon());
if (verbose)
logInform("Constraint %s:: Joint name: '%s', actual value: %f, desired value: %f, tolerance_above: %f, tolerance_below: %f",
result ? "satisfied" : "violated", joint_variable_name_.c_str(),
current_joint_position, joint_position_, joint_tolerance_above_, joint_tolerance_below_);
return ConstraintEvaluationResult(result, constraint_weight_ * fabs(dif));
}
kinematic_constraints::ConstraintEvaluationResult kinematic_constraints::OrientationConstraint::decide(const robot_state::RobotState &state, bool verbose) const
{
if (!link_model_)
return ConstraintEvaluationResult(true, 0.0);
const robot_state::LinkState *link_state = state.getLinkState(link_model_->getName());
if (!link_state)
{
logWarn("No link in state with name '%s'", link_model_->getName().c_str());
return ConstraintEvaluationResult(false, 0.0);
}
Eigen::Vector3d xyz;
if (mobile_frame_)
{
Eigen::Matrix3d tmp;
tf_->transformRotationMatrix(state, desired_rotation_frame_id_, desired_rotation_matrix_, tmp);
Eigen::Affine3d diff(tmp.inverse() * link_state->getGlobalLinkTransform().rotation());
xyz = diff.rotation().eulerAngles(0, 1, 2);
// 0,1,2 corresponds to ZXZ, the convention used in sampling constraints
}
else
{
Eigen::Affine3d diff(desired_rotation_matrix_inv_ * link_state->getGlobalLinkTransform().rotation());
xyz = diff.rotation().eulerAngles(0, 1, 2); // 0,1,2 corresponds to ZXZ, the convention used in sampling constraints
}
xyz(0) = std::min(fabs(xyz(0)), boost::math::constants::pi<double>() - fabs(xyz(0)));
xyz(1) = std::min(fabs(xyz(1)), boost::math::constants::pi<double>() - fabs(xyz(1)));
xyz(2) = std::min(fabs(xyz(2)), boost::math::constants::pi<double>() - fabs(xyz(2)));
bool result = xyz(2) < absolute_z_axis_tolerance_+std::numeric_limits<double>::epsilon()
&& xyz(1) < absolute_y_axis_tolerance_+std::numeric_limits<double>::epsilon()
&& xyz(0) < absolute_x_axis_tolerance_+std::numeric_limits<double>::epsilon();
if (verbose)
{
Eigen::Quaterniond q_act(link_state->getGlobalLinkTransform().rotation());
Eigen::Quaterniond q_des(desired_rotation_matrix_);
logInform("Orientation constraint %s for link '%s'. Quaternion desired: %f %f %f %f, quaternion actual: %f %f %f %f, error: x=%f, y=%f, z=%f, tolerance: x=%f, y=%f, z=%f",
result ? "satisfied" : "violated", link_model_->getName().c_str(),
q_des.x(), q_des.y(), q_des.z(), q_des.w(),
q_act.x(), q_act.y(), q_act.z(), q_act.w(), xyz(0), xyz(1), xyz(2),
absolute_x_axis_tolerance_, absolute_y_axis_tolerance_, absolute_z_axis_tolerance_);
}
return ConstraintEvaluationResult(result, constraint_weight_ * (xyz(0) + xyz(1) + xyz(2)));
}
bool constraint_samplers::UnionConstraintSampler::sample(robot_state::RobotState &state, const robot_state::RobotState &reference_state, unsigned int max_attempts)
{
state = reference_state;
state.setToRandomPositions(jmg_);
if (samplers_.size() >= 1)
{
if (!samplers_[0]->sample(state, reference_state, max_attempts))
return false;
}
for (std::size_t i = 1 ; i < samplers_.size() ; ++i)
if (!samplers_[i]->sample(state, state, max_attempts))
return false;
return true;
}
bool KinematicsMetrics::getManipulabilityEllipsoid(const robot_state::RobotState &state,
const robot_model::JointModelGroup *joint_model_group,
Eigen::MatrixXcd &eigen_values,
Eigen::MatrixXcd &eigen_vectors) const
{
// state.getJacobian() only works for chain groups.
if(!joint_model_group->isChain())
{
return false;
}
Eigen::MatrixXd jacobian = state.getJacobian(joint_model_group);
Eigen::MatrixXd matrix = jacobian*jacobian.transpose();
Eigen::EigenSolver<Eigen::MatrixXd> eigensolver(matrix.block(0, 0, 3, 3));
eigen_values = eigensolver.eigenvalues();
eigen_vectors = eigensolver.eigenvectors();
return true;
}
moveit_msgs::Constraints kinematic_constraints::constructGoalConstraints(const robot_state::RobotState &state, const robot_model::JointModelGroup *jmg,
double tolerance_below, double tolerance_above)
{
moveit_msgs::Constraints goal;
std::vector<double> vals;
state.copyJointGroupPositions(jmg, vals);
goal.joint_constraints.resize(vals.size());
for (std::size_t i = 0 ; i < vals.size() ; ++i)
{
goal.joint_constraints[i].joint_name = jmg->getVariableNames()[i];
goal.joint_constraints[i].position = vals[i];
goal.joint_constraints[i].tolerance_above = tolerance_below;
goal.joint_constraints[i].tolerance_below = tolerance_above;
goal.joint_constraints[i].weight = 1.0;
}
return goal;
}
double KinematicsMetrics::getJointLimitsPenalty(const robot_state::RobotState &state, const robot_model::JointModelGroup *joint_model_group) const
{
if (fabs(penalty_multiplier_) <= boost::math::tools::epsilon<double>())
return 1.0;
double joint_limits_multiplier(1.0);
const std::vector<const robot_model::JointModel*> &joint_model_vector = joint_model_group->getJointModels();
for (std::size_t i = 0; i < joint_model_vector.size(); ++i)
{
if (joint_model_vector[i]->getType() == robot_model::JointModel::REVOLUTE)
{
const robot_model::RevoluteJointModel* revolute_model = static_cast<const robot_model::RevoluteJointModel*>(joint_model_vector[i]);
if (revolute_model->isContinuous())
continue;
}
if (joint_model_vector[i]->getType() == robot_model::JointModel::PLANAR)
{
const robot_model::JointModel::Bounds &bounds = joint_model_vector[i]->getVariableBounds();
if (bounds[0].min_position_ == -std::numeric_limits<double>::max() || bounds[0].max_position_ == std::numeric_limits<double>::max() ||
bounds[1].min_position_ == -std::numeric_limits<double>::max() || bounds[1].max_position_ == std::numeric_limits<double>::max() ||
bounds[2].min_position_ == -boost::math::constants::pi<double>() || bounds[2].max_position_ == boost::math::constants::pi<double>())
continue;
}
if (joint_model_vector[i]->getType() == robot_model::JointModel::FLOATING)
{
//Joint limits are not well-defined for floating joints
continue;
}
const double *joint_values = state.getJointPositions(joint_model_vector[i]);
const robot_model::JointModel::Bounds &bounds = joint_model_vector[i]->getVariableBounds();
std::vector<double> lower_bounds, upper_bounds;
for (std::size_t j = 0; j < bounds.size(); ++j)
{
lower_bounds.push_back(bounds[j].min_position_);
upper_bounds.push_back(bounds[j].max_position_);
}
double lower_bound_distance = joint_model_vector[i]->distance(joint_values, &lower_bounds[0]);
double upper_bound_distance = joint_model_vector[i]->distance(joint_values, &upper_bounds[0]);
double range = lower_bound_distance + upper_bound_distance;
if (range <= boost::math::tools::epsilon<double>())
continue;
joint_limits_multiplier *= (lower_bound_distance * upper_bound_distance/(range*range));
}
return (1.0 - exp(-penalty_multiplier_*joint_limits_multiplier));
}
void constraint_samplers::visualizeDistribution(const ConstraintSamplerPtr& sampler,
const robot_state::RobotState& reference_state,
const std::string& link_name, unsigned int sample_count,
visualization_msgs::MarkerArray& markers)
{
if (!sampler)
{
ROS_ERROR_NAMED("constraint_samplers", "No sampler specified for visualizing distribution of samples");
return;
}
const robot_state::LinkModel* lm = reference_state.getLinkModel(link_name);
if (!lm)
return;
robot_state::RobotState ks(reference_state);
std_msgs::ColorRGBA color;
color.r = 1.0f;
color.g = 0.0f;
color.b = 0.0f;
color.a = 1.0f;
for (unsigned int i = 0; i < sample_count; ++i)
{
if (!sampler->sample(ks))
continue;
const Eigen::Vector3d& pos = ks.getGlobalLinkTransform(lm).translation();
visualization_msgs::Marker mk;
mk.header.stamp = ros::Time::now();
mk.header.frame_id = sampler->getJointModelGroup()->getParentModel().getModelFrame();
mk.ns = "constraint_samples";
mk.id = i;
mk.type = visualization_msgs::Marker::SPHERE;
mk.action = visualization_msgs::Marker::ADD;
mk.pose.position.x = pos.x();
mk.pose.position.y = pos.y();
mk.pose.position.z = pos.z();
mk.pose.orientation.w = 1.0;
mk.scale.x = mk.scale.y = mk.scale.z = 0.035;
mk.color = color;
mk.lifetime = ros::Duration(30.0);
markers.markers.push_back(mk);
}
}
kinematic_constraints::ConstraintEvaluationResult kinematic_constraints::PositionConstraint::decide(const robot_state::RobotState &state, bool verbose) const
{
if (!link_model_ || constraint_region_.empty())
return ConstraintEvaluationResult(true, 0.0);
const robot_state::LinkState *link_state = state.getLinkState(link_model_->getName());
if (!link_state)
{
logWarn("No link in state with name '%s'", link_model_->getName().c_str());
return ConstraintEvaluationResult(false, 0.0);
}
Eigen::Vector3d pt = link_state->getGlobalLinkTransform() * offset_;
if (mobile_frame_)
{
Eigen::Affine3d tmp;
for (std::size_t i = 0 ; i < constraint_region_.size() ; ++i)
{
tf_->transformPose(state, constraint_frame_id_, constraint_region_pose_[i], tmp);
bool result = constraint_region_[i]->cloneAt(tmp)->containsPoint(pt, verbose);
if (result || (i + 1 == constraint_region_pose_.size()))
return finishPositionConstraintDecision(pt, tmp.translation(), link_model_->getName(), constraint_weight_, result, verbose);
else
finishPositionConstraintDecision(pt, tmp.translation(), link_model_->getName(), constraint_weight_, result, verbose);
}
}
else
{
for (std::size_t i = 0 ; i < constraint_region_.size() ; ++i)
{
bool result = constraint_region_[i]->containsPoint(pt, true);
if (result || (i + 1 == constraint_region_.size()))
return finishPositionConstraintDecision(pt, constraint_region_[i]->getPose().translation(), link_model_->getName(), constraint_weight_, result, verbose);
else
finishPositionConstraintDecision(pt, constraint_region_[i]->getPose().translation(), link_model_->getName(), constraint_weight_, result, verbose);
}
}
return ConstraintEvaluationResult(false, 0.0);
}
void displayInitialWaypoints(robot_state::RobotState& state,
ros::NodeHandle& node_handle,
robot_model::RobotModelPtr& robot_model,
const std::vector<std::string>& hierarchy,
const std::vector<Eigen::VectorXd>& waypoints)
{
static ros::Publisher vis_marker_array_publisher = node_handle.advertise<visualization_msgs::MarkerArray>("/move_itomp/visualization_marker_array", 10);
visualization_msgs::MarkerArray ma;
std::vector<std::string> link_names = robot_model->getLinkModelNames();
std_msgs::ColorRGBA color;
color.a = 0.5;
color.r = 1.0;
color.g = 1.0;
color.b = 0.0;
//ros::Duration dur(3600.0);
ros::Duration dur(0.25);
for (unsigned int point = 0; point < waypoints.size(); ++point)
{
ma.markers.clear();
setRobotStateFrom(state, hierarchy, waypoints, point);
double time = 0.05;
ros::WallDuration timer(time);
timer.sleep();
std::string ns = "init_" + boost::lexical_cast<std::string>(point);
state.getRobotMarkers(ma, link_names, color, ns, dur);
vis_marker_array_publisher.publish(ma);
}
}
bool userCallback(const robot_state::RobotState &kinematic_state, bool verbose)
{
// get the joint value for the right shoulder pan of the PR2 robot
const std::vector<double>& joint_state_values = kinematic_state.getJointState("shoulder_pan_joint")->getVariableValues();
return (joint_state_values.front() > 0.0);
}
void planning_scene_monitor::CurrentStateMonitor::setToCurrentState(robot_state::RobotState &upd) const
{
boost::mutex::scoped_lock slock(state_update_lock_);
const double *pos = robot_state_.getVariablePositions();
upd.setVariablePositions(pos);
}
void ompl_interface::ModelBasedStateSpace::copyToOMPLState(ompl::base::State *state, const robot_state::RobotState &rstate) const
{
rstate.copyJointGroupPositions(spec_.joint_model_group_, state->as<StateType>()->values);
// clear any cached info (such as validity known or not)
state->as<StateType>()->clearKnownInformation();
}
void ompl_interface::ModelBasedStateSpace::copyToRobotState(robot_state::RobotState& rstate, const ompl::base::State *state) const
{
rstate.setJointGroupPositions(spec_.joint_model_group_, state->as<StateType>()->values);
rstate.update();
}
// BEGIN_SUB_TUTORIAL userCallback
//
// User defined constraints can also be specified to the PlanningScene
// class. This is done by specifying a callback using the
// setStateFeasibilityPredicate function. Here's a simple example of a
// user-defined callback that checks whether the "r_shoulder_pan" of
// the PR2 robot is at a positive or negative angle:
bool userCallback(const robot_state::RobotState &kinematic_state, bool verbose)
{
const double* joint_values = kinematic_state.getJointPositions("shoulder_pan_joint");
return (joint_values[0] > 0.0);
}
void MotionPlanningFrame::updateQueryStateHelper(robot_state::RobotState &state, const std::string &v)
{
if (v == "<random>")
{
configureWorkspace();
if (const robot_model::JointModelGroup *jmg = state.getJointModelGroup(planning_display_->getCurrentPlanningGroup()))
state.setToRandomPositions(jmg);
}
else
if (v == "<random valid>")
{
configureWorkspace();
if (const robot_model::JointModelGroup *jmg =
state.getJointModelGroup(planning_display_->getCurrentPlanningGroup()))
{
// Loop until a collision free state is found
static const int MAX_ATTEMPTS = 100;
int attempt_count = 0; // prevent loop for going forever
while (attempt_count < MAX_ATTEMPTS)
{
// Generate random state
state.setToRandomPositions(jmg);
state.update(); // prevent dirty transforms
// Test for collision
if (planning_display_->getPlanningSceneRO()->isStateValid(state, "", false))
break;
attempt_count ++;
}
// Explain if no valid rand state found
if (attempt_count >= MAX_ATTEMPTS)
ROS_WARN("Unable to find a random collision free configuration after %d attempts", MAX_ATTEMPTS);
}
else
{
ROS_WARN_STREAM("Unable to get joint model group " << planning_display_->getCurrentPlanningGroup());
}
}
else
if (v == "<current>")
{
const planning_scene_monitor::LockedPlanningSceneRO &ps = planning_display_->getPlanningSceneRO();
if (ps)
state = ps->getCurrentState();
}
else
if (v == "<same as goal>")
{
state = *planning_display_->getQueryGoalState();
}
else
if (v == "<same as start>")
{
state = *planning_display_->getQueryStartState();
}
else
{
// maybe it is a named state
if (const robot_model::JointModelGroup *jmg = state.getJointModelGroup(planning_display_->getCurrentPlanningGroup()))
state.setToDefaultValues(jmg, v);
}
}