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));
}
//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;
}