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 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 MotionPlanningFrame::appendStateToAction(apc_msgs::PrimitiveAction& action,
const robot_state::RobotState& state)
{
APC_ASSERT(!action.group_id.empty(), "Failed to find group in action");
const moveit::core::JointModelGroup* jmg = state.getJointModelGroup(action.group_id);
APC_ASSERT(jmg, "Failed to get joint model group %s", action.group_id.c_str());
// Append joint angles of state to the joint trajectory.
std::vector<std::string> joint_names = jmg->getVariableNames(); //jmg->getActiveJointModelNames();
std::vector<std::string> variable_names = jmg->getVariableNames();
// Set joint variable names if they are missing.
if (action.joint_trajectory.joint_names.size() == 0) {
action.joint_trajectory.joint_names = joint_names;
} else if (action.joint_trajectory.joint_names.size() != joint_names.size()) {
action.joint_trajectory.joint_names = joint_names;
}
action.joint_trajectory.joint_names = joint_names; // HACK
// Append joint angles of state to the joint trajectory.
Eigen::VectorXd variable_values;
state.copyJointGroupPositions(jmg, variable_values);
trajectory_msgs::JointTrajectoryPoint point;
int appended = 0;
for (int i = 0; i < variable_values.size(); i++) {
if (std::find(joint_names.begin(), joint_names.end(), variable_names[i]) != joint_names.end()) {
point.positions.push_back(variable_values[i]);
appended++;
}
}
action.joint_trajectory.points.push_back(point);
// Assert that the number of joint names in the input action
// match the number positions in the point.
APC_ASSERT(appended == action.joint_trajectory.joint_names.size(),
"Mismatch in number of positions when appending state to action");
// Assert that the joint names match the active joints of the
// group.
const std::vector<std::string>& action_joint_names = action.joint_trajectory.joint_names;
for (int i = 0; i < action_joint_names.size(); i++)
APC_ASSERT(std::find(joint_names.begin(), joint_names.end(), action_joint_names[i]) != joint_names.end(),
"Failed to find joint %s in group %s", action_joint_names[i].c_str(), action.group_id.c_str());
for (int i = 0; i < joint_names.size(); i++)
APC_ASSERT(std::find(action_joint_names.begin(), action_joint_names.end(), joint_names[i]) != action_joint_names.end(),
"Failed to find joint %s in group %s", joint_names[i].c_str(), action.group_id.c_str());
}
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;
}
