void planning_environment::getAllKinematicStateStampedTransforms(const planning_models::KinematicState& state,
std::vector<geometry_msgs::TransformStamped>& trans_vector,
const ros::Time& stamp)
{
trans_vector.clear();
for(unsigned int i = 0; i < state.getLinkStateVector().size(); i++) {
const planning_models::KinematicState::LinkState* ls = state.getLinkStateVector()[i];
geometry_msgs::TransformStamped ts;
ts.header.stamp = stamp;
ts.header.frame_id = state.getKinematicModel()->getRoot()->getParentFrameId();
ts.child_frame_id = ls->getName();
if(ts.header.frame_id == ts.child_frame_id) continue;
tf::transformTFToMsg(ls->getGlobalLinkTransform(),ts.transform);
trans_vector.push_back(ts);
}
}
void collision_detection::CollisionRobotFCL::constructFCLObject(const planning_models::KinematicState &state, FCLObject &fcl_obj) const
{
const std::vector<planning_models::KinematicState::LinkState*> &link_states = state.getLinkStateVector();
fcl_obj.collision_objects_.reserve(geoms_.size());
for (std::size_t i = 0 ; i < geoms_.size() ; ++i)
{
if (geoms_[i] && geoms_[i]->collision_geometry_)
{
fcl::CollisionObject *collObj = new fcl::CollisionObject(geoms_[i]->collision_geometry_, transform2fcl(link_states[i]->getGlobalCollisionBodyTransform()));
fcl_obj.collision_objects_.push_back(boost::shared_ptr<fcl::CollisionObject>(collObj));
// the CollisionGeometryData is already stored in the class member geoms_, so we need not copy it
}
std::vector<const planning_models::KinematicState::AttachedBody*> ab;
link_states[i]->getAttachedBodies(ab);
for (std::size_t j = 0 ; j < ab.size() ; ++j)
{
std::vector<FCLGeometryConstPtr> objs;
getAttachedBodyObjects(ab[j], objs);
const EigenSTL::vector_Affine3d &ab_t = ab[j]->getGlobalCollisionBodyTransforms();
for (std::size_t k = 0 ; k < objs.size() ; ++k)
if (objs[k]->collision_geometry_)
{
fcl::CollisionObject *collObj = new fcl::CollisionObject(objs[k]->collision_geometry_, transform2fcl(ab_t[k]));
fcl_obj.collision_objects_.push_back(boost::shared_ptr<fcl::CollisionObject>(collObj));
// we copy the shared ptr to the CollisionGeometryData, as this is not stored by the class itself,
// and would be destroyed when objs goes out of scope.
fcl_obj.collision_geometry_.push_back(objs[k]);
}
}
}
}
void planning_environment::updateAttachedObjectBodyPoses(planning_environment::CollisionModels* cm,
planning_models::KinematicState& state,
tf::TransformListener& tf)
{
cm->bodiesLock();
const std::map<std::string, std::map<std::string, bodies::BodyVector*> >& link_att_objects = cm->getLinkAttachedObjects();
if(link_att_objects.empty()) {
cm->bodiesUnlock();
return;
}
//this gets all the attached bodies in their correct current positions according to tf
geometry_msgs::PoseStamped ps;
ps.pose.orientation.w = 1.0;
for(std::map<std::string, std::map<std::string, bodies::BodyVector*> >::const_iterator it = link_att_objects.begin();
it != link_att_objects.end();
it++) {
ps.header.frame_id = it->first;
std::string es;
if (tf.getLatestCommonTime(cm->getWorldFrameId(), it->first, ps.header.stamp, &es) != tf::NO_ERROR) {
ROS_INFO_STREAM("Problem transforming into fixed frame from " << it->first << ". Error string " << es);
continue;
}
geometry_msgs::PoseStamped psout;
tf.transformPose(cm->getWorldFrameId(), ps, psout);
tf::Transform link_trans;
tf::poseMsgToTF(psout.pose, link_trans);
state.updateKinematicStateWithLinkAt(it->first, link_trans);
cm->updateAttachedBodyPosesForLink(state, it->first);
}
cm->bodiesUnlock();
}
bool planning_environment::KinematicModelStateMonitor::setKinematicStateToTime(const ros::Time& time,
planning_models::KinematicState& state) const
{
std::map<std::string, double> joint_value_map;
if(!getCachedJointStateValues(time, joint_value_map)) {
return false;
}
state.setKinematicState(joint_value_map);
return true;
}
kinematic_constraints::ConstraintEvaluationResult kinematic_constraints::JointConstraint::decide(const planning_models::KinematicState &state, bool verbose) const
{
if (!joint_model_)
return ConstraintEvaluationResult(true, 0.0);
const planning_models::KinematicState::JointState *joint = state.getJointState(joint_model_->getName());
if (!joint)
{
ROS_WARN_STREAM("No joint in state with name '" << joint_model_->getName() << "'");
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())
{
ROS_WARN_STREAM("Local name '" << local_variable_name_ << "' is not known to joint state with name '" << joint_model_->getName() << "'");
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
// however, we want to include proper sign for diff, as the tol below is may be different from tol above
if (current_joint_position < joint_position_)
dif = -dif;
}
else
dif = current_joint_position - joint_position_;
// check bounds
bool result = dif <= joint_tolerance_above_ && dif >= -joint_tolerance_below_;
if (verbose)
ROS_INFO("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));
}
bool world_transform(std::string frame_id,
const planning_models::KinematicState &state,
btTransform &transform) {
if (!frame_id.compare(state.getKinematicModel()->getRoot()->getParentFrameId())) {
//identity transform
transform.setIdentity();
return true;
}
if (!frame_id.compare(state.getKinematicModel()->getRoot()->getChildFrameId())) {
transform = state.getRootTransform();
return true;
}
const planning_models::KinematicState::LinkState *link =
state.getLinkState(frame_id);
if (!link) {
ROS_ERROR("Unable to find link %s in kinematic state", frame_id.c_str());
return false;
}
transform = link->getGlobalLinkTransform();
return true;
}
kinematic_constraints::ConstraintEvaluationResult kinematic_constraints::OrientationConstraint::decide(const planning_models::KinematicState &state, bool verbose) const
{
if (!link_model_)
return ConstraintEvaluationResult(true, 0.0);
const planning_models::KinematicState::LinkState *link_state = state.getLinkState(link_model_->getName());
if (!link_state)
{
ROS_WARN_STREAM("No link in state with name '" << link_model_->getName() << "'");
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); // XYZ
}
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_ && xyz(1) < absolute_y_axis_tolerance_ && xyz(0) < absolute_x_axis_tolerance_;
if (verbose)
{
Eigen::Quaterniond q_act(link_state->getGlobalLinkTransform().rotation());
Eigen::Quaterniond q_des(desired_rotation_matrix_);
ROS_INFO("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 planning_environment::configureForAttachedBodyMask(planning_models::KinematicState& state,
planning_environment::CollisionModels* cm,
tf::TransformListener& tf,
const std::string& sensor_frame,
const ros::Time& sensor_time,
tf::Vector3& sensor_pos)
{
state.setKinematicStateToDefault();
cm->bodiesLock();
const std::map<std::string, std::map<std::string, bodies::BodyVector*> >& link_att_objects = cm->getLinkAttachedObjects();
if(link_att_objects.empty()) {
cm->bodiesUnlock();
return false;
}
planning_environment::updateAttachedObjectBodyPoses(cm,
state,
tf);
sensor_pos.setValue(0.0,0.0,0.0);
// compute the origin of the sensor in the frame of the cloud
if (!sensor_frame.empty()) {
std::string err;
try {
tf::StampedTransform transf;
tf.lookupTransform(cm->getWorldFrameId(), sensor_frame, sensor_time, transf);
sensor_pos = transf.getOrigin();
} catch(tf::TransformException& ex) {
ROS_ERROR("Unable to lookup transform from %s to %s. Exception: %s", sensor_frame.c_str(), cm->getWorldFrameId().c_str(), ex.what());
sensor_pos.setValue(0, 0, 0);
}
}
cm->bodiesUnlock();
return true;
}
void planning_environment::convertKinematicStateToRobotState(const planning_models::KinematicState& kinematic_state,
const ros::Time& timestamp,
const std::string& header_frame,
arm_navigation_msgs::RobotState &robot_state)
{
robot_state.joint_state.position.clear();
robot_state.joint_state.name.clear();
robot_state.multi_dof_joint_state.joint_names.clear();
robot_state.multi_dof_joint_state.frame_ids.clear();
robot_state.multi_dof_joint_state.child_frame_ids.clear();
robot_state.multi_dof_joint_state.poses.clear();
const std::vector<planning_models::KinematicState::JointState*>& joints = kinematic_state.getJointStateVector();
for(std::vector<planning_models::KinematicState::JointState*>::const_iterator it = joints.begin();
it != joints.end();
it++) {
const std::vector<double>& joint_state_values = (*it)->getJointStateValues();
const std::vector<std::string>& joint_state_value_names = (*it)->getJointStateNameOrder();
for(unsigned int i = 0; i < joint_state_values.size(); i++) {
robot_state.joint_state.name.push_back(joint_state_value_names[i]);
robot_state.joint_state.position.push_back(joint_state_values[i]);
}
if(!(*it)->getParentFrameId().empty() && !(*it)->getChildFrameId().empty()) {
robot_state.multi_dof_joint_state.stamp = ros::Time::now();
robot_state.multi_dof_joint_state.joint_names.push_back((*it)->getName());
robot_state.multi_dof_joint_state.frame_ids.push_back((*it)->getParentFrameId());
robot_state.multi_dof_joint_state.child_frame_ids.push_back((*it)->getChildFrameId());
tf::Pose p((*it)->getVariableTransform());
geometry_msgs::Pose pose;
tf::poseTFToMsg(p, pose);
robot_state.multi_dof_joint_state.poses.push_back(pose);
}
}
robot_state.joint_state.header.stamp = timestamp;
robot_state.joint_state.header.frame_id = header_frame;
return;
}
kinematic_constraints::ConstraintEvaluationResult kinematic_constraints::PositionConstraint::decide(const planning_models::KinematicState &state, bool verbose) const
{
if (!link_model_ || constraint_region_.empty())
return ConstraintEvaluationResult(true, 0.0);
const planning_models::KinematicState::LinkState *link_state = state.getLinkState(link_model_->getName());
if (!link_state)
{
ROS_WARN_STREAM("No link in state with name '" << link_model_->getName() << "'");
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);
if (result || (i + 1 == constraint_region_pose_.size()))
return 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);
if (result || (i + 1 == constraint_region_.size()))
return finishPositionConstraintDecision(pt, constraint_region_[i]->getPose().translation(), link_model_->getName(), constraint_weight_, result, verbose);
}
}
return ConstraintEvaluationResult(false, 0.0);
}
bool constraint_samplers::IKConstraintSampler::samplePose(Eigen::Vector3d &pos, Eigen::Quaterniond &quat,
const planning_models::KinematicState &ks,
unsigned int max_attempts)
{
if (sampling_pose_.position_constraint_)
{
const std::vector<bodies::BodyPtr> &b = sampling_pose_.position_constraint_->getConstraintRegions();
if (!b.empty())
{
bool found = false;
std::size_t k = random_number_generator_.uniformInteger(0, b.size() - 1);
for (std::size_t i = 0 ; i < b.size() ; ++i)
if (b[(i+k) % b.size()]->samplePointInside(random_number_generator_, max_attempts, pos))
{
found = true;
break;
}
if (!found)
{
ROS_ERROR("Unable to sample a point inside the constraint region");
return false;
}
}
else
{
ROS_ERROR("Unable to sample a point inside the constraint region. Constraint region is empty when it should not be.");
return false;
}
// if this constraint is with respect a mobile frame, we need to convert this rotation to the root frame of the model
if (sampling_pose_.position_constraint_->mobileReferenceFrame())
{
const planning_models::KinematicState::LinkState *ls = ks.getLinkState(sampling_pose_.position_constraint_->getReferenceFrame());
pos = ls->getGlobalLinkTransform() * pos;
}
}
else
{
// do FK for rand state
planning_models::KinematicState tempState(ks);
planning_models::KinematicState::JointStateGroup *tmp = tempState.getJointStateGroup(jmg_->getName());
if (tmp)
{
tmp->setToRandomValues();
pos = tempState.getLinkState(sampling_pose_.orientation_constraint_->getLinkModel()->getName())->getGlobalLinkTransform().translation();
}
else
{
ROS_ERROR("Passed a JointStateGroup for a mismatching JointModelGroup");
return false;
}
}
if (sampling_pose_.orientation_constraint_)
{
// sample a rotation matrix within the allowed bounds
double angle_x = 2.0 * (random_number_generator_.uniform01() - 0.5) * sampling_pose_.orientation_constraint_->getXAxisTolerance();
double angle_y = 2.0 * (random_number_generator_.uniform01() - 0.5) * sampling_pose_.orientation_constraint_->getYAxisTolerance();
double angle_z = 2.0 * (random_number_generator_.uniform01() - 0.5) * sampling_pose_.orientation_constraint_->getZAxisTolerance();
Eigen::Affine3d diff(Eigen::AngleAxisd(angle_x, Eigen::Vector3d::UnitX())
* Eigen::AngleAxisd(angle_y, Eigen::Vector3d::UnitY())
* Eigen::AngleAxisd(angle_z, Eigen::Vector3d::UnitZ()));
Eigen::Affine3d reqr(sampling_pose_.orientation_constraint_->getDesiredRotationMatrix() * diff.rotation());
quat = Eigen::Quaterniond(reqr.rotation());
// if this constraint is with respect a mobile frame, we need to convert this rotation to the root frame of the model
if (sampling_pose_.orientation_constraint_->mobileReferenceFrame())
{
const planning_models::KinematicState::LinkState *ls = ks.getLinkState(sampling_pose_.orientation_constraint_->getReferenceFrame());
Eigen::Affine3d rt(ls->getGlobalLinkTransform().rotation() * quat.toRotationMatrix());
quat = Eigen::Quaterniond(rt.rotation());
}
}
else
{
// sample a random orientation
double q[4];
random_number_generator_.quaternion(q);
quat = Eigen::Quaterniond(q[3], q[0], q[1], q[2]);
}
// if there is an offset, we need to undo the induced rotation in the sampled transform origin (point)
if (sampling_pose_.position_constraint_ && sampling_pose_.position_constraint_->hasLinkOffset())
// the rotation matrix that corresponds to the desired orientation
pos = pos - quat.toRotationMatrix() * sampling_pose_.position_constraint_->getLinkOffset();
// we now have the transform we wish to perform IK for, in the planning frame
if (transform_ik_)
{
// we need to convert this transform to the frame expected by the IK solver
// both the planning frame and the frame for the IK are assumed to be robot links
Eigen::Affine3d ikq(Eigen::Translation3d(pos) * quat.toRotationMatrix());
const planning_models::KinematicState::LinkState *ls = ks.getLinkState(ik_frame_);
ikq = ls->getGlobalLinkTransform().inverse() * ikq;
pos = ikq.translation();
quat = Eigen::Quaterniond(ikq.rotation());
}
return true;
}
//returns true if the joint_state_map sets all the joints in the state,
bool planning_environment::setRobotStateAndComputeTransforms(const arm_navigation_msgs::RobotState &robot_state,
planning_models::KinematicState& state)
{
if(robot_state.joint_state.name.size() != robot_state.joint_state.position.size()) {
ROS_INFO_STREAM("Different number of names and positions: " << robot_state.joint_state.name.size()
<< " " << robot_state.joint_state.position.size());
return false;
}
std::map<std::string, double> joint_state_map;
for (unsigned int i = 0 ; i < robot_state.joint_state.name.size(); ++i)
{
joint_state_map[robot_state.joint_state.name[i]] = robot_state.joint_state.position[i];
}
std::vector<std::string> missing_states;
bool complete = state.setKinematicState(joint_state_map, missing_states);
if(!complete) {
if(missing_states.empty()) {
ROS_INFO("Incomplete, but no missing states");
}
}
std::map<std::string, bool> has_missing_state_map;
for(unsigned int i = 0; i < missing_states.size(); i++) {
has_missing_state_map[missing_states[i]] = false;
}
bool need_to_update = false;
if(robot_state.multi_dof_joint_state.joint_names.size() > 1) {
ROS_INFO("More than 1 joint names");
}
for(unsigned int i = 0; i < robot_state.multi_dof_joint_state.joint_names.size(); i++) {
std::string joint_name = robot_state.multi_dof_joint_state.joint_names[i];
if(!state.hasJointState(joint_name)) {
ROS_WARN_STREAM("No joint matching multi-dof joint " << joint_name);
continue;
}
planning_models::KinematicState::JointState* joint_state = state.getJointState(joint_name);
if(robot_state.multi_dof_joint_state.frame_ids.size() <= i) {
ROS_INFO_STREAM("Robot state msg had bad multi_dof transform - not enough frame ids");
} else if(robot_state.multi_dof_joint_state.child_frame_ids.size() <= i) {
ROS_INFO_STREAM("Robot state msg had bad multi_dof transform - not enough child frame ids");
} else if(robot_state.multi_dof_joint_state.frame_ids[i] != joint_state->getParentFrameId() ||
robot_state.multi_dof_joint_state.child_frame_ids[i] != joint_state->getChildFrameId()) {
ROS_WARN_STREAM("Robot state msg has bad multi_dof transform - frame ids or child frame_ids do not match up with joint");
} else {
tf::StampedTransform transf;
tf::poseMsgToTF(robot_state.multi_dof_joint_state.poses[i], transf);
joint_state->setJointStateValues(transf);
need_to_update = true;
//setting true for all individual joint names because we're setting the transform
for(unsigned int i = 0; i < joint_state->getJointStateNameOrder().size(); i++) {
has_missing_state_map[joint_state->getJointStateNameOrder()[i]] = true;
}
}
}
if(need_to_update) {
state.updateKinematicLinks();
}
if(!complete) {
for(std::map<std::string, bool>::iterator it = has_missing_state_map.begin();
it != has_missing_state_map.end();
it++) {
if(!it->second) {
return false;
}
}
return true;
}
return true;
}
void planning_environment::KinematicModelStateMonitor::setStateValuesFromCurrentValues(planning_models::KinematicState& state) const
{
current_joint_values_lock_.lock();
state.setKinematicState(current_joint_state_map_);
current_joint_values_lock_.unlock();
}
