45 #include <boost/bind.hpp> 58 const std::string
LOGNAME =
"robot_state";
61 : robot_model_(robot_model)
62 , has_velocity_(false)
63 , has_acceleration_(false)
65 , dirty_link_transforms_(robot_model_->getRootJoint())
66 , dirty_collision_body_transforms_(nullptr)
90 static_assert((
sizeof(Eigen::Isometry3d) / EIGEN_MAX_ALIGN_BYTES) * EIGEN_MAX_ALIGN_BYTES ==
sizeof(Eigen::Isometry3d),
91 "sizeof(Eigen::Isometry3d) should be a multiple of EIGEN_MAX_ALIGN_BYTES");
93 constexpr
unsigned int extra_alignment_bytes = EIGEN_MAX_ALIGN_BYTES - 1;
95 const int nr_doubles_for_dirty_joint_transforms =
96 1 +
robot_model_->getJointModelCount() / (
sizeof(double) /
sizeof(
unsigned char));
100 sizeof(
double) * (
robot_model_->getVariableCount() * 3 + nr_doubles_for_dirty_joint_transforms) +
101 extra_alignment_bytes;
107 ~(uintptr_t)extra_alignment_bytes);
121 const int nr_doubles_for_dirty_joint_transforms =
122 1 +
robot_model_->getJointModelCount() / (
sizeof(double) /
sizeof(
unsigned char));
160 const int nr_doubles_for_dirty_joint_transforms =
161 1 +
robot_model_->getJointModelCount() / (
sizeof(double) /
sizeof(
unsigned char));
168 nr_doubles_for_dirty_joint_transforms);
174 for (std::map<std::string, AttachedBody*>::const_iterator it = other.
attached_body_map_.begin();
176 attachBody(it->second->getName(), it->second->getShapes(), it->second->getFixedTransforms(),
177 it->second->getTouchLinks(), it->second->getAttachedLinkName(), it->second->getDetachPosture());
184 ROS_WARN_NAMED(LOGNAME,
"Returning dirty joint transforms for joint '%s'", joint->
getName().c_str());
204 ROS_WARN_NAMED(LOGNAME,
"Returning dirty collision body transforms");
298 for (std::size_t i = 0; i < joints.size(); ++i)
299 joints[i]->getVariableRandomPositions(rng,
position_ + joints[i]->getFirstVariableIndex());
304 const std::vector<double>& distances)
310 assert(distances.size() == joints.size());
311 for (std::size_t i = 0; i < joints.size(); ++i)
313 const int idx = joints[i]->getFirstVariableIndex();
314 joints[i]->getVariableRandomPositionsNearBy(rng,
position_ + joints[i]->getFirstVariableIndex(),
326 for (std::size_t i = 0; i < joints.size(); ++i)
328 const int idx = joints[i]->getFirstVariableIndex();
329 joints[i]->getVariableRandomPositionsNearBy(rng,
position_ + joints[i]->getFirstVariableIndex(),
337 std::map<std::string, double> m;
366 for (std::map<std::string, double>::const_iterator it = variable_map.begin(), end = variable_map.end(); it != end;
378 std::vector<std::string>& missing_variables)
const 380 missing_variables.clear();
381 const std::vector<std::string>& nm =
robot_model_->getVariableNames();
382 for (std::size_t i = 0; i < nm.size(); ++i)
383 if (variable_map.find(nm[i]) == variable_map.end())
384 if (
robot_model_->getJointOfVariable(nm[i])->getMimic() ==
nullptr)
385 missing_variables.push_back(nm[i]);
389 std::vector<std::string>& missing_variables)
396 const std::vector<double>& variable_position)
398 for (std::size_t i = 0; i < variable_names.size(); ++i)
411 for (std::map<std::string, double>::const_iterator it = variable_map.begin(), end = variable_map.end(); it != end;
417 std::vector<std::string>& missing_variables)
424 const std::vector<double>& variable_velocity)
427 assert(variable_names.size() == variable_velocity.size());
428 for (std::size_t i = 0; i < variable_names.size(); ++i)
435 for (std::map<std::string, double>::const_iterator it = variable_map.begin(), end = variable_map.end(); it != end;
441 std::vector<std::string>& missing_variables)
448 const std::vector<double>& variable_acceleration)
451 assert(variable_names.size() == variable_acceleration.size());
452 for (std::size_t i = 0; i < variable_names.size(); ++i)
459 for (std::map<std::string, double>::const_iterator it = variable_map.begin(), end = variable_map.end(); it != end;
465 std::vector<std::string>& missing_variables)
472 const std::vector<double>& variable_effort)
475 assert(variable_names.size() == variable_effort.size());
476 for (std::size_t i = 0; i < variable_names.size(); ++i)
484 for (
size_t i = 0; i <
robot_model_->getVariableCount(); ++i)
493 ROS_ERROR_NAMED(LOGNAME,
"Unable to set joint efforts because array is being used for accelerations");
508 for (std::size_t i = 0; i < il.size(); ++i)
517 for (std::size_t i = 0; i < il.size(); ++i)
528 for (std::size_t i = 0; i < il.size(); ++i)
535 values.resize(il.size());
536 for (std::size_t i = 0; i < il.size(); ++i)
548 for (std::size_t i = 0; i < il.size(); ++i)
557 for (std::size_t i = 0; i < il.size(); ++i)
567 for (std::size_t i = 0; i < il.size(); ++i)
574 values.resize(il.size());
575 for (std::size_t i = 0; i < il.size(); ++i)
587 for (std::size_t i = 0; i < il.size(); ++i)
596 for (std::size_t i = 0; i < il.size(); ++i)
606 for (std::size_t i = 0; i < il.size(); ++i)
613 values.resize(il.size());
614 for (std::size_t i = 0; i < il.size(); ++i)
644 const std::vector<int>& ot_id = link->areCollisionOriginTransformsIdentity();
645 const int index_co = link->getFirstCollisionBodyTransformIndex();
646 const int index_l = link->getLinkIndex();
647 for (std::size_t j = 0, end = ot.size(); j != end; ++j)
677 int idx_link = link->getLinkIndex();
682 if (link->parentJointIsFixed())
687 if (link->jointOriginTransformIsIdentity())
698 if (link->jointOriginTransformIsIdentity())
702 link->getJointOriginTransform().affine() *
getJointTransform(link->getParentJointModel()).matrix();
707 for (std::map<std::string, AttachedBody*>::const_iterator it =
attached_body_map_.begin();
727 for (std::size_t i = 0; i < cj.size(); ++i)
737 child_link = parent_link;
750 for (std::size_t i = 0; i < cj.size(); ++i)
759 for (std::map<std::string, AttachedBody*>::const_iterator it =
attached_body_map_.begin();
766 const std::vector<const JointModel*>& jm =
robot_model_->getActiveJointModels();
767 for (std::size_t i = 0; i < jm.size(); ++i)
776 for (std::size_t i = 0; i < jm.size(); ++i)
784 const std::vector<const JointModel*>& jm =
robot_model_->getActiveJointModels();
785 for (std::size_t i = 0; i < jm.size(); ++i)
792 for (std::size_t i = 0; i < jm.size(); ++i)
818 std::pair<double, const JointModel*>
821 double distance = std::numeric_limits<double>::max();
823 for (std::size_t i = 0; i < joints.size(); ++i)
828 if (static_cast<const RevoluteJointModel*>(joints[i])->isContinuous())
833 std::vector<double> lower_bounds(bounds.size()), upper_bounds(bounds.size());
834 for (std::size_t j = 0; j < bounds.size(); ++j)
836 lower_bounds[j] = bounds[j].min_position_;
837 upper_bounds[j] = bounds[j].max_position_;
839 double new_distance = joints[i]->distance(joint_values, &lower_bounds[0]);
840 if (new_distance < distance)
843 distance = new_distance;
845 new_distance = joints[i]->
distance(joint_values, &upper_bounds[0]);
846 if (new_distance < distance)
849 distance = new_distance;
852 return std::make_pair(distance, index);
858 for (std::size_t joint_id = 0; joint_id < jm.size(); ++joint_id)
860 const int idx = jm[joint_id]->getFirstVariableIndex();
861 const std::vector<VariableBounds>& bounds = jm[joint_id]->getVariableBounds();
864 for (std::size_t var_id = 0; var_id < jm[joint_id]->getVariableCount(); ++var_id)
868 if (dtheta > dt * bounds[var_id].max_velocity_)
879 for (std::size_t i = 0; i < jm.size(); ++i)
881 const int idx = jm[i]->getFirstVariableIndex();
882 d += jm[i]->getDistanceFactor() * jm[i]->distance(
position_ + idx, other.
position_ + idx);
898 for (std::size_t i = 0; i < jm.size(); ++i)
900 const int idx = jm[i]->getFirstVariableIndex();
918 std::map<std::string, AttachedBody*>::const_iterator it =
attached_body_map_.find(
id);
941 const std::string& link,
const trajectory_msgs::JointTrajectory& detach_posture)
953 attached_bodies.clear();
955 for (std::map<std::string, AttachedBody*>::const_iterator it =
attached_body_map_.begin();
957 attached_bodies.push_back(it->second);
962 attached_bodies.clear();
963 for (std::map<std::string, AttachedBody*>::const_iterator it =
attached_body_map_.begin();
965 if (group->
hasLinkModel(it->second->getAttachedLinkName()))
966 attached_bodies.push_back(it->second);
971 attached_bodies.clear();
972 for (std::map<std::string, AttachedBody*>::const_iterator it =
attached_body_map_.begin();
974 if (it->second->getAttachedLink() == lm)
975 attached_bodies.push_back(it->second);
980 for (std::map<std::string, AttachedBody*>::const_iterator it =
attached_body_map_.begin();
995 if (it->second->getAttachedLink() != link)
1003 std::map<std::string, AttachedBody*>::iterator
del = it++;
1013 if (!group->
hasLinkModel(it->second->getAttachedLinkName()))
1021 std::map<std::string, AttachedBody*>::iterator
del = it++;
1049 if (!frame_id.empty() && frame_id[0] ==
'/')
1053 static const Eigen::Isometry3d IDENTITY_TRANSFORM = Eigen::Isometry3d::Identity();
1055 return IDENTITY_TRANSFORM;
1061 std::map<std::string, AttachedBody*>::const_iterator jt =
attached_body_map_.find(frame_id);
1065 "Transform from frame '%s' to frame '%s' is not known " 1066 "('%s' should be a link name or an attached body's id).",
1067 frame_id.c_str(),
robot_model_->getModelFrame().c_str(), frame_id.c_str());
1068 return IDENTITY_TRANSFORM;
1073 ROS_ERROR_NAMED(LOGNAME,
"Attached body '%s' has no geometry associated to it. No transform to return.",
1075 return IDENTITY_TRANSFORM;
1079 "There are multiple geometries associated to attached body '%s'. " 1080 "Returning the transform for the first one.",
1087 if (!frame_id.empty() && frame_id[0] ==
'/')
1091 std::map<std::string, AttachedBody*>::const_iterator it =
attached_body_map_.find(frame_id);
1092 return it !=
attached_body_map_.end() && !it->second->getGlobalCollisionBodyTransforms().empty();
1096 const std_msgs::ColorRGBA& color,
const std::string& ns,
const ros::Duration& dur,
1097 bool include_attached)
const 1099 std::size_t cur_num = arr.markers.size();
1101 unsigned int id = cur_num;
1102 for (std::size_t i = cur_num; i < arr.markers.size(); ++i, ++id)
1104 arr.markers[i].ns = ns;
1105 arr.markers[i].id = id;
1106 arr.markers[i].lifetime = dur;
1107 arr.markers[i].color = color;
1112 bool include_attached)
const 1115 for (std::size_t i = 0; i < link_names.size(); ++i)
1117 ROS_DEBUG_NAMED(LOGNAME,
"Trying to get marker for link '%s'", link_names[i].c_str());
1121 if (include_attached)
1122 for (std::map<std::string, AttachedBody*>::const_iterator it =
attached_body_map_.begin();
1124 if (it->second->getAttachedLink() == lm)
1126 for (std::size_t j = 0; j < it->second->getShapes().size(); ++j)
1128 visualization_msgs::Marker att_mark;
1129 att_mark.header.frame_id =
robot_model_->getModelFrame();
1130 att_mark.header.stamp = tm;
1134 if (fabs(att_mark.scale.x * att_mark.scale.y * att_mark.scale.z) < std::numeric_limits<float>::epsilon())
1136 att_mark.pose =
tf2::toMsg(it->second->getGlobalCollisionBodyTransforms()[j]);
1137 arr.markers.push_back(att_mark);
1145 for (std::size_t j = 0; j < lm->
getShapes().size(); ++j)
1147 visualization_msgs::Marker mark;
1149 mark.header.stamp = tm;
1153 if (mesh_resource.empty() || lm->
getShapes().size() > 1)
1158 if (fabs(mark.scale.x * mark.scale.y * mark.scale.z) < std::numeric_limits<float>::epsilon())
1164 mark.type = mark.MESH_RESOURCE;
1165 mark.mesh_use_embedded_materials =
false;
1166 mark.mesh_resource = mesh_resource;
1169 mark.scale.x = mesh_scale[0];
1170 mark.scale.y = mesh_scale[1];
1171 mark.scale.z = mesh_scale[2];
1175 arr.markers.push_back(mark);
1183 Eigen::MatrixXd result;
1185 throw Exception(
"Unable to compute Jacobian");
1190 const Eigen::Vector3d& reference_point_position, Eigen::MatrixXd& jacobian,
1191 bool use_quaternion_representation)
const 1197 ROS_ERROR_NAMED(LOGNAME,
"The group '%s' is not a chain. Cannot compute Jacobian.", group->
getName().c_str());
1203 ROS_ERROR_NAMED(LOGNAME,
"Link name '%s' does not exist in the chain '%s' or is not a child for this chain",
1210 Eigen::Isometry3d reference_transform =
1212 int rows = use_quaternion_representation ? 7 : 6;
1214 jacobian = Eigen::MatrixXd::Zero(rows, columns);
1217 Eigen::Vector3d point_transform = link_transform * reference_point_position;
1227 Eigen::Isometry3d joint_transform;
1251 jacobian.block<3, 1>(0, joint_index) =
1252 jacobian.block<3, 1>(0, joint_index) + joint_axis.cross(point_transform - joint_transform.translation());
1253 jacobian.block<3, 1>(3, joint_index) = jacobian.block<3, 1>(3, joint_index) + joint_axis;
1259 jacobian.block<3, 1>(0, joint_index) = jacobian.block<3, 1>(0, joint_index) + joint_axis;
1265 jacobian.block<3, 1>(0, joint_index) = jacobian.block<3, 1>(0, joint_index) + joint_axis;
1267 jacobian.block<3, 1>(0, joint_index + 1) = jacobian.block<3, 1>(0, joint_index + 1) + joint_axis;
1269 jacobian.block<3, 1>(0, joint_index + 2) = jacobian.block<3, 1>(0, joint_index + 2) +
1270 joint_axis.cross(point_transform - joint_transform.translation());
1271 jacobian.block<3, 1>(3, joint_index + 2) = jacobian.block<3, 1>(3, joint_index + 2) + joint_axis;
1274 ROS_ERROR_NAMED(LOGNAME,
"Unknown type of joint in Jacobian computation");
1276 if (pjm == root_joint_model)
1280 if (use_quaternion_representation)
1287 Eigen::Quaterniond q(link_transform.rotation());
1288 double w = q.w(),
x = q.x(),
y = q.y(),
z = q.z();
1289 Eigen::MatrixXd quaternion_update_matrix(4, 3);
1290 quaternion_update_matrix << -
x, -
y, -
z, w, -z, y, z, w, -x, -y, x, w;
1291 jacobian.block(3, 0, 4, columns) = 0.5 * quaternion_update_matrix * jacobian.block(3, 0, 3, columns);
1299 Eigen::VectorXd qdot;
1307 Eigen::Matrix<double, 6, 1>
t;
1313 const Eigen::VectorXd& twist,
const LinkModel* tip)
const 1322 Eigen::MatrixXd e_wb = Eigen::ArrayXXd::Zero(6, 6);
1323 e_wb.block(0, 0, 3, 3) = e_mb.matrix().block(0, 0, 3, 3);
1324 e_wb.block(3, 3, 3, 3) = e_mb.matrix().block(0, 0, 3, 3);
1328 Eigen::JacobiSVD<Eigen::MatrixXd> svd_of_j(j, Eigen::ComputeThinU | Eigen::ComputeThinV);
1329 const Eigen::MatrixXd& u = svd_of_j.matrixU();
1330 const Eigen::MatrixXd& v = svd_of_j.matrixV();
1331 const Eigen::VectorXd&
s = svd_of_j.singularValues();
1333 Eigen::VectorXd sinv = s;
1334 static const double PINVTOLER = std::numeric_limits<float>::epsilon();
1336 for (std::size_t i = 0; i < static_cast<std::size_t>(s.rows()); ++i)
1337 if (fabs(s(i)) > maxsv)
1339 for (std::size_t i = 0; i < static_cast<std::size_t>(s.rows()); ++i)
1342 if (fabs(s(i)) > maxsv * PINVTOLER)
1343 sinv(i) = 1.0 / s(i);
1347 Eigen::MatrixXd jinv = (v * sinv.asDiagonal() * u.transpose());
1350 qdot = jinv * twist;
1364 std::vector<double>
values;
1366 return constraint(
this, jmg, &values[0]);
1382 return setFromIK(jmg, pose, solver->getTipFrame(), timeout, constraint, options);
1389 Eigen::Isometry3d mat;
1391 static std::vector<double> consistency_limits;
1392 return setFromIK(jmg, mat, tip, consistency_limits, timeout, constraint, options);
1405 static std::vector<double> consistency_limits;
1406 return setFromIK(jmg, pose, solver->getTipFrame(), consistency_limits, timeout, constraint, options);
1413 static std::vector<double> consistency_limits;
1414 return setFromIK(jmg, pose_in, tip_in, consistency_limits, timeout, constraint, options);
1421 const std::vector<double>& ik_sol, moveit_msgs::MoveItErrorCodes& error_code)
1424 std::vector<double> solution(bij.size());
1425 for (std::size_t i = 0; i < bij.size(); ++i)
1426 solution[bij[i]] = ik_sol[i];
1427 if (constraint(state, group, &solution[0]))
1428 error_code.val = moveit_msgs::MoveItErrorCodes::SUCCESS;
1445 const LinkModel* lm =
getLinkModel((!ik_frame.empty() && ik_frame[0] ==
'/') ? ik_frame.substr(1) : ik_frame);
1457 const std::vector<double>& consistency_limits_in,
double timeout,
1463 poses.push_back(pose_in);
1465 std::vector<std::string> tips;
1466 tips.push_back(tip_in);
1468 std::vector<std::vector<double> > consistency_limits;
1469 consistency_limits.push_back(consistency_limits_in);
1471 return setFromIK(jmg, poses, tips, consistency_limits, timeout, constraint, options);
1475 const std::vector<std::string>& tips_in,
double timeout,
1479 const std::vector<std::vector<double> > consistency_limits;
1480 return setFromIK(jmg, poses_in, tips_in, consistency_limits, timeout, constraint, options);
1484 const std::vector<std::string>& tips_in,
1485 const std::vector<std::vector<double> >& consistency_limit_sets,
double timeout,
1490 if (poses_in.size() != tips_in.size())
1492 ROS_ERROR_NAMED(LOGNAME,
"Number of poses must be the same as number of tips");
1500 bool valid_solver =
true;
1503 valid_solver =
false;
1506 else if (poses_in.size() > 1)
1508 std::string error_msg;
1509 if (!solver->supportsGroup(jmg, &error_msg))
1511 ROS_ERROR_NAMED(LOGNAME,
"Kinematics solver %s does not support joint group %s. Error: %s",
1512 typeid(*solver).name(), jmg->
getName().c_str(), error_msg.c_str());
1513 valid_solver =
false;
1520 if (poses_in.size() > 1)
1523 return setFromIKSubgroups(jmg, poses_in, tips_in, consistency_limit_sets, timeout, constraint, options);
1533 std::vector<double> consistency_limits;
1534 if (consistency_limit_sets.size() > 1)
1537 "Invalid number (%zu) of sets of consistency limits for a setFromIK request " 1538 "that is being solved by a single IK solver",
1539 consistency_limit_sets.size());
1542 else if (consistency_limit_sets.size() == 1)
1543 consistency_limits = consistency_limit_sets[0];
1545 const std::vector<std::string>& solver_tip_frames = solver->getTipFrames();
1548 std::vector<bool> tip_frames_used(solver_tip_frames.size(),
false);
1551 std::vector<geometry_msgs::Pose> ik_queries(solver_tip_frames.size());
1554 for (std::size_t i = 0; i < poses_in.size(); ++i)
1557 Eigen::Isometry3d pose = poses_in[i];
1558 std::string pose_frame = tips_in[i];
1561 if (!pose_frame.empty() && pose_frame[0] ==
'/')
1562 pose_frame = pose_frame.substr(1);
1570 bool found_valid_frame =
false;
1571 std::size_t solver_tip_id;
1572 for (solver_tip_id = 0; solver_tip_id < solver_tip_frames.size(); ++solver_tip_id)
1575 if (tip_frames_used[solver_tip_id])
1579 std::string solver_tip_frame = solver_tip_frames[solver_tip_id];
1583 if (!solver_tip_frame.empty() && solver_tip_frame[0] ==
'/')
1584 solver_tip_frame = solver_tip_frame.substr(1);
1586 if (pose_frame != solver_tip_frame)
1592 if (ab_trans.size() != 1)
1595 "with multiple geometries as a reference frame.");
1599 pose = pose * ab_trans[0].inverse();
1601 if (pose_frame != solver_tip_frame)
1610 for (robot_model::LinkTransformMap::const_iterator it = fixed_links.begin(); it != fixed_links.end(); ++it)
1613 pose_frame = solver_tip_frame;
1614 pose = pose * it->second;
1622 if (pose_frame == solver_tip_frame)
1624 found_valid_frame =
true;
1631 if (!found_valid_frame)
1633 ROS_ERROR_NAMED(LOGNAME,
"Cannot compute IK for query %zu pose reference frame '%s'", i, pose_frame.c_str());
1635 std::stringstream ss;
1636 for (solver_tip_id = 0; solver_tip_id < solver_tip_frames.size(); ++solver_tip_id)
1637 ss << solver_tip_frames[solver_tip_id] <<
", ";
1638 ROS_ERROR_NAMED(LOGNAME,
"Available tip frames: [%s]", ss.str().c_str());
1643 tip_frames_used[solver_tip_id] =
true;
1646 geometry_msgs::Pose ik_query;
1650 ik_queries[solver_tip_id] = ik_query;
1654 for (std::size_t solver_tip_id = 0; solver_tip_id < solver_tip_frames.size(); ++solver_tip_id)
1657 if (tip_frames_used[solver_tip_id])
1661 std::string solver_tip_frame = solver_tip_frames[solver_tip_id];
1665 if (!solver_tip_frame.empty() && solver_tip_frame[0] ==
'/')
1666 solver_tip_frame = solver_tip_frame.substr(1);
1676 geometry_msgs::Pose ik_query;
1680 ik_queries[solver_tip_id] = ik_query;
1683 tip_frames_used[solver_tip_id] =
true;
1687 if (timeout < std::numeric_limits<double>::epsilon())
1693 ik_callback_fn = boost::bind(&ikCallbackFnAdapter,
this, jmg, constraint, _1, _2, _3);
1698 std::vector<double> initial_values;
1700 std::vector<double> seed(bij.size());
1701 for (std::size_t i = 0; i < bij.size(); ++i)
1702 seed[i] = initial_values[bij[i]];
1705 std::vector<double> ik_sol;
1706 moveit_msgs::MoveItErrorCodes error;
1708 if (solver->searchPositionIK(ik_queries, seed, timeout, consistency_limits, ik_sol, ik_callback_fn, error, options,
1711 std::vector<double> solution(bij.size());
1712 for (std::size_t i = 0; i < bij.size(); ++i)
1713 solution[bij[i]] = ik_sol[i];
1721 const std::vector<std::string>& tips_in,
1722 const std::vector<std::vector<double> >& consistency_limits,
double timeout,
1729 std::vector<const JointModelGroup*> sub_groups;
1733 if (poses_in.size() != sub_groups.size())
1735 ROS_ERROR_NAMED(LOGNAME,
"Number of poses (%zu) must be the same as number of sub-groups (%zu)", poses_in.size(),
1740 if (tips_in.size() != sub_groups.size())
1742 ROS_ERROR_NAMED(LOGNAME,
"Number of tip names (%zu) must be same as number of sub-groups (%zu)", tips_in.size(),
1747 if (!consistency_limits.empty() && consistency_limits.size() != sub_groups.size())
1749 ROS_ERROR_NAMED(LOGNAME,
"Number of consistency limit vectors must be the same as number of sub-groups");
1753 for (std::size_t i = 0; i < consistency_limits.size(); ++i)
1755 if (consistency_limits[i].size() != sub_groups[i]->getVariableCount())
1757 ROS_ERROR_NAMED(LOGNAME,
"Number of joints in consistency_limits is %zu but it should be should be %u", i,
1764 std::vector<kinematics::KinematicsBaseConstPtr> solvers;
1765 for (std::size_t i = 0; i < poses_in.size(); ++i)
1767 kinematics::KinematicsBaseConstPtr solver = sub_groups[i]->getSolverInstance();
1773 solvers.push_back(solver);
1778 std::vector<std::string> pose_frames = tips_in;
1781 for (std::size_t i = 0; i < poses_in.size(); ++i)
1783 Eigen::Isometry3d& pose = transformed_poses[i];
1784 std::string& pose_frame = pose_frames[i];
1791 std::string solver_tip_frame = solvers[i]->getTipFrame();
1795 if (!solver_tip_frame.empty() && solver_tip_frame[0] ==
'/')
1796 solver_tip_frame = solver_tip_frame.substr(1);
1798 if (pose_frame != solver_tip_frame)
1804 if (ab_trans.size() != 1)
1806 ROS_ERROR_NAMED(LOGNAME,
"Cannot use an attached body with multiple geometries as a reference frame.");
1810 pose = pose * ab_trans[0].inverse();
1812 if (pose_frame != solver_tip_frame)
1818 for (robot_model::LinkTransformMap::const_iterator it = fixed_links.begin(); it != fixed_links.end(); ++it)
1819 if (it->first->getName() == solver_tip_frame)
1821 pose_frame = solver_tip_frame;
1822 pose = pose * it->second;
1828 if (pose_frame != solver_tip_frame)
1830 ROS_ERROR_NAMED(LOGNAME,
"Cannot compute IK for query pose reference frame '%s', desired: '%s'",
1831 pose_frame.c_str(), solver_tip_frame.c_str());
1837 std::vector<geometry_msgs::Pose> ik_queries(poses_in.size());
1840 ik_callback_fn = boost::bind(&ikCallbackFnAdapter,
this, jmg, constraint, _1, _2, _3);
1842 for (std::size_t i = 0; i < transformed_poses.size(); ++i)
1844 Eigen::Quaterniond quat(transformed_poses[i].rotation());
1846 ik_queries[i].position.x = point.x();
1847 ik_queries[i].position.y = point.y();
1848 ik_queries[i].position.z = point.z();
1849 ik_queries[i].orientation.x = quat.x();
1850 ik_queries[i].orientation.y = quat.y();
1851 ik_queries[i].orientation.z = quat.z();
1852 ik_queries[i].orientation.w = quat.w();
1856 if (timeout < std::numeric_limits<double>::epsilon())
1861 bool first_seed =
true;
1862 unsigned int attempts = 0;
1867 bool found_solution =
true;
1868 for (std::size_t sg = 0; sg < sub_groups.size(); ++sg)
1870 const std::vector<unsigned int>& bij = sub_groups[sg]->getKinematicsSolverJointBijection();
1871 std::vector<double> seed(bij.size());
1875 std::vector<double> initial_values;
1877 for (std::size_t i = 0; i < bij.size(); ++i)
1878 seed[i] = initial_values[bij[i]];
1884 std::vector<double> random_values;
1885 sub_groups[sg]->getVariableRandomPositions(rng, random_values);
1886 for (std::size_t i = 0; i < bij.size(); ++i)
1887 seed[i] = random_values[bij[i]];
1891 std::vector<double> ik_sol;
1892 moveit_msgs::MoveItErrorCodes error;
1893 const std::vector<double>& climits = consistency_limits.empty() ? std::vector<double>() : consistency_limits[sg];
1894 if (solvers[sg]->searchPositionIK(ik_queries[sg], seed, (timeout - elapsed) / sub_groups.size(), climits, ik_sol,
1897 std::vector<double> solution(bij.size());
1898 for (std::size_t i = 0; i < bij.size(); ++i)
1899 solution[bij[i]] = ik_sol[i];
1904 found_solution =
false;
1910 std::vector<double> full_solution;
1912 if (constraint ? constraint(
this, jmg, &full_solution[0]) :
true)
1920 }
while (elapsed < timeout);
1935 const Eigen::Vector3d rotated_direction = global_reference_frame ? direction : start_pose.rotation() * direction;
1938 Eigen::Isometry3d target_pose = start_pose;
1939 target_pose.translation() += rotated_direction *
distance;
1943 computeCartesianPath(group, traj, link, target_pose,
true, max_step, jump_threshold, validCallback, options));
1947 const LinkModel* link,
const Eigen::Isometry3d& target,
1948 bool global_reference_frame,
const MaxEEFStep& max_step,
1955 for (std::size_t i = 0; i < cjnt.size(); ++i)
1962 Eigen::Isometry3d rotated_target = global_reference_frame ? target : start_pose * target;
1964 Eigen::Quaterniond start_quaternion(start_pose.rotation());
1965 Eigen::Quaterniond target_quaternion(rotated_target.rotation());
1970 "Invalid MaxEEFStep passed into computeCartesianPath. Both the MaxEEFStep.rotation and " 1971 "MaxEEFStep.translation components must be non-negative and at least one component must be " 1972 "greater than zero");
1976 double rotation_distance = start_quaternion.angularDistance(target_quaternion);
1977 double translation_distance = (rotated_target.translation() - start_pose.translation()).norm();
1980 std::size_t translation_steps = 0;
1982 translation_steps = floor(translation_distance / max_step.
translation);
1984 std::size_t rotation_steps = 0;
1986 rotation_steps = floor(rotation_distance / max_step.
rotation);
1989 std::size_t steps = std::max(translation_steps, rotation_steps) + 1;
1990 if (jump_threshold.
factor > 0 && steps < MIN_STEPS_FOR_JUMP_THRESH)
1994 std::vector<double> consistency_limits;
1999 switch (jm->getType())
2011 limit = jm->getMaximumExtent();
2012 consistency_limits.push_back(limit);
2016 traj.push_back(RobotStatePtr(
new RobotState(*
this)));
2018 double last_valid_percentage = 0.0;
2019 for (std::size_t i = 1; i <= steps; ++i)
2021 double percentage = (double)i / (
double)steps;
2023 Eigen::Isometry3d pose(start_quaternion.slerp(percentage, target_quaternion));
2024 pose.translation() = percentage * rotated_target.translation() + (1 - percentage) * start_pose.translation();
2028 if (
setFromIK(group, pose, link->
getName(), consistency_limits, 0.0, validCallback, options))
2029 traj.push_back(RobotStatePtr(
new RobotState(*
this)));
2033 last_valid_percentage = percentage;
2038 return last_valid_percentage;
2043 bool global_reference_frame,
const MaxEEFStep& max_step,
2048 double percentage_solved = 0.0;
2049 for (std::size_t i = 0; i < waypoints.size(); ++i)
2053 std::vector<RobotStatePtr> waypoint_traj;
2054 double wp_percentage_solved =
computeCartesianPath(group, waypoint_traj, link, waypoints[i], global_reference_frame,
2055 max_step, NO_JOINT_SPACE_JUMP_TEST, validCallback, options);
2056 if (fabs(wp_percentage_solved - 1.0) < std::numeric_limits<double>::epsilon())
2058 percentage_solved = (double)(i + 1) / (double)waypoints.size();
2059 std::vector<RobotStatePtr>::iterator
start = waypoint_traj.begin();
2060 if (i > 0 && !waypoint_traj.empty())
2061 std::advance(start, 1);
2062 traj.insert(traj.end(), start, waypoint_traj.end());
2066 percentage_solved += wp_percentage_solved / (double)waypoints.size();
2067 std::vector<RobotStatePtr>::iterator
start = waypoint_traj.begin();
2068 if (i > 0 && !waypoint_traj.empty())
2069 std::advance(start, 1);
2070 traj.insert(traj.end(), start, waypoint_traj.end());
2077 return percentage_solved;
2083 double percentage_solved = 1.0;
2084 if (traj.size() <= 1)
2085 return percentage_solved;
2087 if (jump_threshold.
factor > 0.0)
2093 return percentage_solved;
2097 double jump_threshold_factor)
2102 "The computed trajectory is too short to detect jumps in joint-space " 2103 "Need at least %zu steps, only got %zu. Try a lower max_step.",
2104 MIN_STEPS_FOR_JUMP_THRESH, traj.size());
2107 std::vector<double> dist_vector;
2108 dist_vector.reserve(traj.size() - 1);
2109 double total_dist = 0.0;
2110 for (std::size_t i = 1; i < traj.size(); ++i)
2112 double dist_prev_point = traj[i]->distance(*traj[i - 1], group);
2113 dist_vector.push_back(dist_prev_point);
2114 total_dist += dist_prev_point;
2117 double percentage = 1.0;
2119 double thres = jump_threshold_factor * (total_dist / (double)dist_vector.size());
2120 for (std::size_t i = 0; i < dist_vector.size(); ++i)
2121 if (dist_vector[i] > thres)
2123 ROS_DEBUG_NAMED(LOGNAME,
"Truncating Cartesian path due to detected jump in joint-space distance");
2124 percentage = (double)(i + 1) / (double)(traj.size());
2133 double revolute_threshold,
double prismatic_threshold)
2140 LimitData data[2] = { { revolute_threshold, revolute_threshold > 0.0 },
2141 { prismatic_threshold, prismatic_threshold > 0.0 } };
2142 bool still_valid =
true;
2144 for (std::size_t traj_ix = 0, ix_end = traj.size() - 1; traj_ix != ix_end; ++traj_ix)
2146 for (
auto& joint : joints)
2148 unsigned int type_index;
2149 switch (joint->getType())
2159 "Joint %s has not supported type %s. \n" 2160 "testAbsoluteJointSpaceJump only supports prismatic and revolute joints.",
2161 joint->getName().c_str(), joint->getTypeName().c_str());
2164 if (!data[type_index].check_)
2167 double distance = traj[traj_ix]->distance(*traj[traj_ix + 1], joint);
2168 if (distance > data[type_index].limit_)
2170 ROS_DEBUG_NAMED(LOGNAME,
"Truncating Cartesian path due to detected jump of %.4f > %.4f in joint %s", distance,
2171 data[type_index].limit_, joint->getName().c_str());
2172 still_valid =
false;
2179 double percent_valid = (double)(traj_ix + 1) / (double)(traj.size());
2180 traj.resize(traj_ix + 1);
2181 return percent_valid;
2192 std::vector<const LinkModel*> links =
robot_model_->getLinkModelsWithCollisionGeometry();
2193 for (std::size_t i = 0; i < links.size(); ++i)
2197 transform.translate(links[i]->getCenteredBoundingBoxOffset());
2200 for (std::map<std::string, AttachedBody*>::const_iterator it =
attached_body_map_.begin();
2204 const std::vector<shapes::ShapeConstPtr>&
shapes = it->second->getShapes();
2205 for (std::size_t i = 0; i < transforms.size(); ++i)
2213 aabb.resize(6, 0.0);
2214 if (!bounding_box.isEmpty())
2218 Eigen::Map<Eigen::VectorXd, Eigen::Unaligned, Eigen::InnerStride<2> >(aabb.data(), 3) = bounding_box.min();
2219 Eigen::Map<Eigen::VectorXd, Eigen::Unaligned, Eigen::InnerStride<2> >(aabb.data() + 1, 3) = bounding_box.max();
2225 const std::vector<std::string>& nm =
robot_model_->getVariableNames();
2226 for (std::size_t i = 0; i < nm.size(); ++i)
2227 out << nm[i] <<
"=" <<
position_[i] << std::endl;
2238 for (std::size_t i = 0; i < joints.size(); ++i)
2254 out <<
" " << std::fixed << std::setprecision(5) << bound.
min_position_ <<
"\t";
2256 double step = delta / 20.0;
2258 bool marker_shown =
false;
2262 if (!marker_shown && current_value < value)
2265 marker_shown =
true;
2274 out <<
" \t" << std::fixed << std::setprecision(5) << bound.
max_position_ <<
" \t" << joints[i]->getName()
2275 <<
" current: " << std::fixed << std::setprecision(5) << current_value << std::endl;
2284 out <<
" * Dirty Joint Transforms: " << std::endl;
2285 const std::vector<const JointModel*>& jm =
robot_model_->getJointModels();
2286 for (std::size_t i = 0; i < jm.size(); ++i)
2288 out <<
" " << jm[i]->
getName() << std::endl;
2291 out <<
" * Dirty Collision Body Transforms: " 2297 out <<
"Robot State @" <<
this << std::endl;
2302 out <<
" * Position: ";
2303 for (std::size_t i = 0; i < n; ++i)
2308 out <<
" * Position: NULL" << std::endl;
2312 out <<
" * Velocity: ";
2313 for (std::size_t i = 0; i < n; ++i)
2318 out <<
" * Velocity: NULL" << std::endl;
2322 out <<
" * Acceleration: ";
2323 for (std::size_t i = 0; i < n; ++i)
2328 out <<
" * Acceleration: NULL" << std::endl;
2332 out <<
" * Dirty Collision Body Transforms: " 2340 Eigen::Quaterniond q(transform.rotation());
2341 out <<
"T.xyz = [" << transform.translation().x() <<
", " << transform.translation().y() <<
", " 2342 << transform.translation().z() <<
"], Q.xyzw = [" << q.x() <<
", " << q.y() <<
", " << q.z() <<
", " << q.w()
2343 <<
"]" << std::endl;
2350 out <<
"No transforms computed" << std::endl;
2354 out <<
"Joint transforms:" << std::endl;
2355 const std::vector<const JointModel*>& jm =
robot_model_->getJointModels();
2356 for (std::size_t i = 0; i < jm.size(); ++i)
2358 out <<
" " << jm[i]->getName();
2359 const int idx = jm[i]->getJointIndex();
2366 out <<
"Link poses:" << std::endl;
2367 const std::vector<const LinkModel*>& lm =
robot_model_->getLinkModels();
2368 for (std::size_t i = 0; i < lm.size(); ++i)
2370 out <<
" " << lm[i]->getName() <<
": ";
2377 std::stringstream ss;
2378 ss <<
"ROBOT: " <<
robot_model_->getName() << std::endl;
2385 void getPoseString(std::ostream& ss,
const Eigen::Isometry3d& pose,
const std::string& pfx)
2388 for (
int y = 0;
y < 4; ++
y)
2391 for (
int x = 0;
x < 4; ++
x)
2393 ss << std::setw(8) << pose(
y,
x) <<
" ";
2403 std::string pfx = pfx0 +
"+--";
2405 ss << pfx <<
"Joint: " << jm->
getName() << std::endl;
2407 pfx = pfx0 + (last ?
" " :
"| ");
2417 ss << pfx <<
"Link: " << lm->
getName() << std::endl;
2425 for (std::vector<const JointModel*>::const_iterator it = lm->
getChildJointModels().begin();
random_numbers::RandomNumberGenerator & getRandomNumberGenerator()
Return the instance of a random number generator.
std::pair< double, const JointModel * > getMinDistanceToPositionBounds() const
Get the minimm distance from this state to the bounds. The minimum distance and the joint for which t...
A set of options for the kinematics solver.
Eigen::Isometry3d * variable_joint_transforms_
void updateMimicJoint(const JointModel *joint)
void zeroVelocities()
Set all velocities to 0.0.
bool knowsFrameTransform(const std::string &frame_id) const
Check if a transformation matrix from the model frame to frame frame_id is known. ...
const Eigen::Isometry3d & getVisualMeshOrigin() const
Get the transform for the visual mesh origin.
Core components of MoveIt!
const std::vector< shapes::ShapeConstPtr > & getShapes() const
Get shape associated to the collision geometry for this link.
bool checkJointTransforms(const JointModel *joint) const
This function is only called in debug mode.
void setJointGroupAccelerations(const std::string &joint_group_name, const double *gstate)
Given accelerations for the variables that make up a group, in the order found in the group (includin...
std::map< const LinkModel *, Eigen::Isometry3d, std::less< const LinkModel * >, Eigen::aligned_allocator< std::pair< const LinkModel *const, Eigen::Isometry3d > > > LinkTransformMap
Map from link model instances to Eigen transforms.
const Eigen::Isometry3d & getJointOriginTransform() const
When transforms are computed for this link, they are usually applied to the link's origin...
#define MOVEIT_CONSOLE_COLOR_RESET
double getVariablePosition(const std::string &variable) const
Get the position of a particular variable. An exception is thrown if the variable is not known...
bool setFromIKSubgroups(const JointModelGroup *group, const EigenSTL::vector_Isometry3d &poses, const std::vector< std::string > &tips, const std::vector< std::vector< double > > &consistency_limits, double timeout=0.0, const GroupStateValidityCallbackFn &constraint=GroupStateValidityCallbackFn(), const kinematics::KinematicsQueryOptions &options=kinematics::KinematicsQueryOptions())
setFromIK for multiple poses and tips (end effectors) when no solver exists for the jmg that can solv...
void harmonizePosition(const JointModel *joint)
#define ROS_ERROR_STREAM_NAMED(name, args)
void setJointEfforts(const JointModel *joint, const double *effort)
Vec3fX< details::Vec3Data< double > > Vector3d
#define ROS_WARN_NAMED(name,...)
void updateMimicJoints(const JointModelGroup *group)
Update all mimic joints within group.
const std::vector< unsigned int > & getKinematicsSolverJointBijection() const
Return the mapping between the order of the joints in this group and the order of the joints in the k...
std::vector< double > values
bool dirtyJointTransform(const JointModel *joint) const
bool hasJointModel(const std::string &joint) const
Check if a joint is part of this group.
int getJointIndex() const
Get the index of this joint within the robot model.
const JointModel * getParentJointModel() const
Get the joint model whose child this link is. There will always be a parent joint.
bool hasAttachedBody(const std::string &id) const
Check if an attached body named id exists in this state.
int getLinkIndex() const
The index of this joint when traversing the kinematic tree in depth first fashion.
bool setToIKSolverFrame(Eigen::Isometry3d &pose, const kinematics::KinematicsBaseConstPtr &solver)
Convert the frame of reference of the pose to that same frame as the IK solver expects.
void printDirtyInfo(std::ostream &out=std::cout) const
void getSubgroups(std::vector< const JointModelGroup *> &sub_groups) const
Get the groups that are subsets of this one (in terms of joints set)
void setVariableVelocities(const double *velocity)
Given an array with velocity values for all variables, set those values as the velocities in this sta...
void attachBody(AttachedBody *attached_body)
Add an attached body to this state. Ownership of the memory for the attached body is assumed by the s...
bool isValidVelocityMove(const RobotState &other, const JointModelGroup *group, double dt) const
Check that the time to move between two waypoints is sufficient given velocity limits and time step...
const JointModel * dirty_collision_body_transforms_
const Eigen::Isometry3d & getJointTransform(const std::string &joint_name)
void clearAttachedBodies()
Clear all attached bodies. This calls delete on the AttachedBody instances, if needed.
ROSCONSOLE_CONSOLE_IMPL_DECL std::string getName(void *handle)
void updateLinkTransformsInternal(const JointModel *start)
const std::string & getAttachedLinkName() const
Get the name of the link this body is attached to.
const std::string & getVisualMeshFilename() const
Get the filename of the mesh resource used for visual display of this link.
void harmonizePositions()
Call harmonizePosition() for all joints / all joints in group / given joint.
const AttachedBody * getAttachedBody(const std::string &name) const
Get the attached body named name. Return NULL if not found.
virtual void computeTransform(const double *joint_values, Eigen::Isometry3d &transf) const =0
Given the joint values for a joint, compute the corresponding transform.
bool checkCollisionTransforms() const
This function is only called in debug mode.
void copyJointGroupAccelerations(const std::string &joint_group_name, std::vector< double > &gstate) const
For a given group, copy the acceleration values of the variables that make up the group into another ...
Eigen::Isometry3d * global_link_transforms_
const LinkTransformMap & getAssociatedFixedTransforms() const
Get the set of links that are attached to this one via fixed transforms.
void setJointGroupVelocities(const std::string &joint_group_name, const double *gstate)
Given velocities for the variables that make up a group, in the order found in the group (including v...
static const int NO_IK_SOLUTION
double * getVariablePositions()
Get a raw pointer to the positions of the variables stored in this state. Use carefully. If you change these values externally you need to make sure you trigger a forced update for the state by calling update(true).
void printStatePositionsWithJointLimits(const moveit::core::JointModelGroup *jmg, std::ostream &out=std::cout) const
Output to console the current state of the robot's joint limits.
void setVariablePositions(const double *position)
It is assumed positions is an array containing the new positions for all variables in this state...
geometry_msgs::TransformStamped t
void markDirtyJointTransforms(const JointModel *joint)
static double testAbsoluteJointSpaceJump(const JointModelGroup *group, std::vector< RobotStatePtr > &traj, double revolute_jump_threshold, double prismatic_jump_threshold)
Tests for absolute joint space jumps of the trajectory traj.
const Eigen::Vector3d & getVisualMeshScale() const
Get the scale of the mesh resource for this link.
const LinkModel * getAttachedLink() const
Get the model of the link this body is attached to.
void setAttachedBodyUpdateCallback(const AttachedBodyCallback &callback)
bool dirtyLinkTransforms() const
void dropVelocities()
Remove velocities from this state (this differs from setting them to zero)
void computeVariableVelocity(const JointModelGroup *jmg, Eigen::VectorXd &qdot, const Eigen::VectorXd &twist, const LinkModel *tip) const
Given a twist for a particular link (tip), compute the corresponding velocity for every variable and ...
bool isChain() const
Check if this group is a linear chain.
void dropDynamics()
Reduce RobotState to kinematic information (remove velocity, acceleration and effort, if present)
bool isContiguousWithinState() const
void zeroAccelerations()
Set all accelerations to 0.0.
void setToDefaultValues()
Set all joints to their default positions. The default position is 0, or if that is not within bounds...
void copyJointGroupVelocities(const std::string &joint_group_name, std::vector< double > &gstate) const
For a given group, copy the velocity values of the variables that make up the group into another loca...
void computeAABB(std::vector< double > &aabb) const
Compute an axis-aligned bounding box that contains the current state. The format for aabb is (minx...
const std::vector< std::string > & getVariableNames() const
Get the names of the variables that make up this joint, in the order they appear in corresponding sta...
void copyFrom(const RobotState &other)
#define ROS_DEBUG_NAMED(name,...)
void getAttachedBodies(std::vector< const AttachedBody *> &attached_bodies) const
Get all bodies attached to the model corresponding to this state.
std::string getStateTreeString(const std::string &prefix="") const
void getStateTreeJointString(std::ostream &ss, const JointModel *jm, const std::string &pfx0, bool last) const
bool setFromIK(const JointModelGroup *group, const geometry_msgs::Pose &pose, double timeout=0.0, const GroupStateValidityCallbackFn &constraint=GroupStateValidityCallbackFn(), const kinematics::KinematicsQueryOptions &options=kinematics::KinematicsQueryOptions())
If the group this state corresponds to is a chain and a solver is available, then the joint values ca...
const LinkModel * getParentLinkModel() const
Get the link model whose child this link is (through some joint). There may not always be a parent li...
const std::string & getName() const
Get the name of the attached body.
Struct for containing jump_threshold.
void zeroEffort()
Set all effort values to 0.0.
virtual double distance(const double *value1, const double *value2) const =0
Compute the distance between two joint states of the same model (represented by the variable values) ...
void update(bool force=false)
Update all transforms.
boost::function< void(AttachedBody *body, bool attached)> AttachedBodyCallback
double computeCartesianPath(const JointModelGroup *group, std::vector< RobotStatePtr > &traj, const LinkModel *link, const Eigen::Vector3d &direction, bool global_reference_frame, double distance, const MaxEEFStep &max_step, const JumpThreshold &jump_threshold, const GroupStateValidityCallbackFn &validCallback=GroupStateValidityCallbackFn(), const kinematics::KinematicsQueryOptions &options=kinematics::KinematicsQueryOptions())
Compute the sequence of joint values that correspond to a straight Cartesian path for a particular gr...
void getRobotMarkers(visualization_msgs::MarkerArray &arr, const std::vector< std::string > &link_names, const std_msgs::ColorRGBA &color, const std::string &ns, const ros::Duration &dur, bool include_attached=false) const
Get a MarkerArray that fully describes the robot markers for a given robot.
TF2SIMD_FORCE_INLINE Quaternion inverse(const Quaternion &q)
static double testJointSpaceJump(const JointModelGroup *group, std::vector< RobotStatePtr > &traj, const JumpThreshold &jump_threshold)
Tests joint space jumps of a trajectory.
void setToRandomPositionsNearBy(const JointModelGroup *group, const RobotState &near, double distance)
Set all joints in group to random values near the value in . distance is the maximum amount each join...
unsigned char * dirty_joint_transforms_
void fromMsg(const A &, B &b)
std::size_t getVariableCount() const
Get the number of variables that describe this joint.
const std::string & getName() const
Get the name of the joint group.
This may be thrown if unrecoverable errors occur.
AttachedBodyCallback attached_body_update_callback_
This event is called when there is a change in the attached bodies for this state; The event specifie...
bool getJacobian(const JointModelGroup *group, const LinkModel *link, const Eigen::Vector3d &reference_point_position, Eigen::MatrixXd &jacobian, bool use_quaternion_representation=false) const
Compute the Jacobian with reference to a particular point on a given link, for a specified group...
Struct for containing max_step for computeCartesianPath.
const LinkModel * getChildLinkModel() const
Get the link that this joint connects to. There will always be such a link.
bool integrateVariableVelocity(const JointModelGroup *jmg, const Eigen::VectorXd &qdot, double dt, const GroupStateValidityCallbackFn &constraint=GroupStateValidityCallbackFn())
Given the velocities for the variables in this group (qdot) and an amount of time (dt)...
const kinematics::KinematicsBaseConstPtr getSolverInstance() const
RobotState & operator=(const RobotState &other)
Copy operator.
static double testRelativeJointSpaceJump(const JointModelGroup *group, std::vector< RobotStatePtr > &traj, double jump_threshold_factor)
Tests for relative joint space jumps of the trajectory traj.
const std::string & getName() const
The name of this link.
#define MOVEIT_CONSOLE_COLOR_RED
bool constructMarkerFromShape(const Shape *shape, visualization_msgs::Marker &mk, bool use_mesh_triangle_list=false)
static const std::size_t MIN_STEPS_FOR_JUMP_THRESH
It is recommended that there are at least 10 steps per trajectory for testing jump thresholds with co...
boost::function< void(const geometry_msgs::Pose &ik_pose, const std::vector< double > &ik_solution, moveit_msgs::MoveItErrorCodes &error_code)> IKCallbackFn
Signature for a callback to validate an IK solution. Typically used for collision checking...
A joint from the robot. Models the transform that this joint applies in the kinematic chain...
void interpolate(const RobotState &to, double t, RobotState &state) const
Interpolate from this state towards state to, at time t in [0,1]. The result is stored in state...
void updateStateWithLinkAt(const std::string &link_name, const Eigen::Isometry3d &transform, bool backward=false)
Update the state after setting a particular link to the input global transform pose.
const Eigen::Isometry3d & getFrameTransform(const std::string &frame_id)
Get the transformation matrix from the model frame to the frame identified by frame_id.
bool getVariableDefaultPositions(const std::string &name, std::map< std::string, double > &values) const
Get the values that correspond to a named state as read from the URDF. Return false on failure...
const std::vector< const JointModel * > & getActiveJointModels() const
Get the active joints in this group (that have controllable DOF). This does not include mimic joints...
void copyJointGroupPositions(const std::string &joint_group_name, std::vector< double > &gstate) const
For a given group, copy the position values of the variables that make up the group into another loca...
RobotState(const RobotModelConstPtr &robot_model)
A state can be constructed from a specified robot model. No values are initialized. Call setToDefaultValues() if a state needs to provide valid information.
void printTransforms(std::ostream &out=std::cout) const
int getFirstCollisionBodyTransformIndex() const
const std::string LOGNAME
void printStatePositions(std::ostream &out=std::cout) const
const std::vector< const JointModel * > & getContinuousJointModels() const
Get the array of continuous joints used in this group (may include mimic joints). ...
const std::vector< const JointModel * > & getChildJointModels() const
A link may have 0 or more child joints. From those joints there will certainly be other descendant li...
bool dirtyCollisionBodyTransforms() const
ROSCPP_DECL bool del(const std::string &key)
const LinkModel * getLinkModel(const std::string &link) const
Get the model of a particular link.
void dropEffort()
Remove effort values from this state (this differs from setting them to zero)
int getVariableGroupIndex(const std::string &variable) const
Get the index of a variable within the group. Return -1 on error.
void getMissingKeys(const std::map< std::string, double > &variable_map, std::vector< std::string > &missing_variables) const
bool setFromDiffIK(const JointModelGroup *group, const Eigen::VectorXd &twist, const std::string &tip, double dt, const GroupStateValidityCallbackFn &constraint=GroupStateValidityCallbackFn())
Set the joint values from a Cartesian velocity applied during a time dt.
void printTransform(const Eigen::Isometry3d &transform, std::ostream &out=std::cout) const
const JointModel * dirty_link_transforms_
std::size_t getVariableCount() const
Get the number of variables that make up this state.
bool isLinkUpdated(const std::string &name) const
True if this name is in the set of links that are to be updated when the state of this group changes...
bool satisfiesBounds(double margin=0.0) const
const Eigen::Isometry3d & getGlobalLinkTransform(const std::string &link_name)
void setJointGroupPositions(const std::string &joint_group_name, const double *gstate)
Given positions for the variables that make up a group, in the order found in the group (including va...
random_numbers::RandomNumberGenerator * rng_
For certain operations a state needs a random number generator. However, it may be slightly expensive...
void setToRandomPositions()
Set all joints to random values. Values will be within default bounds.
void dropAccelerations()
Remove accelerations from this state (this differs from setting them to zero)
const std::vector< int > & getVariableIndexList() const
Get the index locations in the complete robot state for all the variables in this group...
RobotModelConstPtr robot_model_
void setVariableAccelerations(const double *acceleration)
Given an array with acceleration values for all variables, set those values as the accelerations in t...
#define ROS_ERROR_NAMED(name,...)
Eigen::Isometry3d * global_collision_body_transforms_
void updateCollisionBodyTransforms()
Update the transforms for the collision bodies. This call is needed before calling collision checking...
void extendWithTransformedBox(const Eigen::Isometry3d &transform, const Eigen::Vector3d &box)
Extend with a box transformed by the given transform.
Representation of a robot's state. This includes position, velocity, acceleration and effort...
void updateLinkTransforms()
Update the reference frame transforms for links. This call is needed before using the transforms of l...
A link from the robot. Contains the constant transform applied to the link and its geometry...
std::vector< Eigen::Isometry3d, Eigen::aligned_allocator< Eigen::Isometry3d > > vector_Isometry3d
bool hasLinkModel(const std::string &link) const
Check if a link is part of this group.
void setVariableEffort(const double *effort)
Given an array with effort values for all variables, set those values as the effort in this state...
unsigned int getVariableCount() const
Get the number of variables that describe this joint group. This includes variables necessary for mim...
int getFirstVariableIndex() const
Get the index of this joint's first variable within the full robot state.
const std::vector< const LinkModel * > & getLinkModels() const
Get the links that are part of this joint group.
Main namespace for MoveIt!
const std::vector< const JointModel * > & getJointModels() const
Get all the joints in this group (including fixed and mimic joints).
Eigen::Vector3d computeShapeExtents(const ShapeMsg &shape_msg)
void computeTransform(const Eigen::Isometry3d &parent_link_global_transform)
Recompute global_collision_body_transform given the transform of the parent link. ...
bool checkLinkTransforms() const
This function is only called in debug mode.
Object defining bodies that can be attached to robot links. This is useful when handling objects pick...
const std::vector< const LinkModel * > & getDescendantLinkModels() const
Get all the link models that descend from this joint, in the kinematic tree.
std::ostream & operator<<(std::ostream &out, const VariableBounds &b)
Operator overload for printing variable bounds to a stream.
std::vector< VariableBounds > Bounds
The datatype for the joint bounds.
double distance(const RobotState &other) const
const LinkModel * getParentLinkModel() const
Get the link that this joint connects to. The robot is assumed to start with a joint, so the root joint will return a NULL pointer here.
JointType getType() const
Get the type of joint.
boost::function< bool(RobotState *robot_state, const JointModelGroup *joint_group, const double *joint_group_variable_values)> GroupStateValidityCallbackFn
Signature for functions that can verify that if the group joint_group in robot_state is set to joint_...
const double * getJointPositions(const std::string &joint_name) const
std::map< std::string, AttachedBody * > attached_body_map_
All attached bodies that are part of this state, indexed by their name.
void printStateInfo(std::ostream &out=std::cout) const
double getDefaultIKTimeout() const
Get the default IK timeout.
const EigenSTL::vector_Isometry3d & getFixedTransforms() const
Get the fixed transform (the transforms to the shapes associated with this body)
bool clearAttachedBody(const std::string &id)
Remove the attached body named id. Return false if the object was not found (and thus not removed)...
void invertVelocity()
Invert velocity if present.
Represents an axis-aligned bounding box.
const std::string & getName() const
Get the name of the joint.