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());
    }
Ejemplo n.º 4
0
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;
}