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 setWalkingStates(robot_state::RobotState& start_state,
robot_state::RobotState& goal_state, Eigen::VectorXd& start_transf,
Eigen::VectorXd& goal_transf, const std::vector<std::string>& hierarchy)
{
std::map<std::string, double> values;
double jointValue = 0.0;
const robot_state::JointModelGroup* joint_model_group =
start_state.getJointModelGroup("whole_body");
joint_model_group->getVariableDefaultPositions("standup", values);
start_state.setVariablePositions(values);
int id = 0;
for(std::vector<std::string>::const_iterator cit = hierarchy.begin();
cit != hierarchy.end(); ++cit, ++id)
{
jointValue = start_transf(id);
start_state.setJointPositions(*cit, &jointValue);
}
goal_state = start_state;
joint_model_group->getVariableDefaultPositions("standup", values);
goal_state.setVariablePositions(values);
id = 0;
for(std::vector<std::string>::const_iterator cit = hierarchy.begin();
cit != hierarchy.end(); ++cit, ++id)
{
jointValue = goal_transf(id);
goal_state.setJointPositions(*cit, &jointValue);
}
}
void doPlan(const std::string& group_name,
planning_interface::MotionPlanRequest& req,
planning_interface::MotionPlanResponse& res,
robot_state::RobotState& start_state,
robot_state::RobotState& goal_state,
planning_scene::PlanningScenePtr& planning_scene,
planning_interface::PlannerManagerPtr& planner_instance)
{
const robot_state::JointModelGroup* joint_model_group =
goal_state.getJointModelGroup("whole_body");
// Copy from start_state to req.start_state
unsigned int num_joints = start_state.getVariableCount();
req.start_state.joint_state.name = start_state.getVariableNames();
req.start_state.joint_state.position.resize(num_joints);
req.start_state.joint_state.velocity.resize(num_joints);
req.start_state.joint_state.effort.resize(num_joints);
memcpy(&req.start_state.joint_state.position[0],
start_state.getVariablePositions(), sizeof(double) * num_joints);
if (start_state.hasVelocities())
memcpy(&req.start_state.joint_state.velocity[0],
start_state.getVariableVelocities(),
sizeof(double) * num_joints);
else
memset(&req.start_state.joint_state.velocity[0], 0,
sizeof(double) * num_joints);
if (start_state.hasAccelerations())
memcpy(&req.start_state.joint_state.effort[0],
start_state.getVariableAccelerations(),
sizeof(double) * num_joints);
else
memset(&req.start_state.joint_state.effort[0], 0,
sizeof(double) * num_joints);
req.group_name = group_name;
moveit_msgs::Constraints joint_goal =
kinematic_constraints::constructGoalConstraints(goal_state,
joint_model_group);
req.goal_constraints.push_back(joint_goal);
// We now construct a planning context that encapsulate the scene,
// the request and the response. We call the planner using this
// planning context
planning_interface::PlanningContextPtr context =
planner_instance->getPlanningContext(planning_scene, req,
res.error_code_);
context->solve(res);
if (res.error_code_.val != res.error_code_.SUCCESS)
{
ROS_ERROR("Could not compute plan successfully");
exit(0);
}
}
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());
}
void setRobotStateFrom(robot_state::RobotState& state,
const std::vector<std::string>& hierarchy,
const std::vector<Eigen::VectorXd>& waypoints,
int index)
{
std::map<std::string, double> values;
double jointValue = 0.0;
const robot_state::JointModelGroup* joint_model_group = state.getJointModelGroup("whole_body");
joint_model_group->getVariableDefaultPositions("standup", values);
state.setVariablePositions(values);
int id = 0;
for(std::vector<std::string>::const_iterator cit = hierarchy.begin(); cit != hierarchy.end(); ++cit, ++id)
{
jointValue = waypoints[index](id);
state.setJointPositions(*cit, &jointValue);
}
}
// This is intended to be called as a ModifyStateFunction to modify the state
// maintained by a LockedRobotState in place.
bool robot_interaction::KinematicOptions::setStateFromIK(
robot_state::RobotState& state,
const std::string& group,
const std::string& tip,
const geometry_msgs::Pose& pose) const
{
const robot_model::JointModelGroup *jmg = state.getJointModelGroup(group);
if (!jmg)
{
ROS_ERROR("No getJointModelGroup('%s') found",group.c_str());
return false;
}
bool result = state.setFromIK(jmg,
pose,
tip,
max_attempts_,
timeout_seconds_,
state_validity_callback_,
options_);
state.update();
return result;
}
void MotionPlanningFrame::computeLinearInterpPlan(const robot_state::RobotState& start,
apc_msgs::PrimitiveAction& goal)
{
std::string group = goal.group_name;
const moveit::core::JointModelGroup* joint_group = start.getJointModelGroup(group);
trajectory_msgs::JointTrajectoryPoint s, c, e;
const std::vector<std::string>& joint_names = goal.joint_trajectory.joint_names;
s = goal.joint_trajectory.points.front();
for (int i = 0; i < joint_names.size(); i++)
s.positions[i] = (start.getVariablePosition(joint_names[i]));
e = goal.joint_trajectory.points.back();
goal.joint_trajectory.points.clear();
trajectory_msgs::JointTrajectory& T = goal.joint_trajectory;
const int n = 15;
c = s;
for (int i = 0; i <= n; i++)
{
for (int j=0; j < c.positions.size(); j++)
c.positions[j] = s.positions[j] + i * (e.positions[j] - s.positions[j]) / n;
T.points.push_back(c);
}
}
void MotionPlanningFrame::updateQueryStateHelper(robot_state::RobotState &state, const std::string &v)
{
if (v == "<random>")
{
configureWorkspace();
if (const robot_model::JointModelGroup *jmg = state.getJointModelGroup(planning_display_->getCurrentPlanningGroup()))
state.setToRandomPositions(jmg);
}
else
if (v == "<random valid>")
{
configureWorkspace();
if (const robot_model::JointModelGroup *jmg =
state.getJointModelGroup(planning_display_->getCurrentPlanningGroup()))
{
// Loop until a collision free state is found
static const int MAX_ATTEMPTS = 100;
int attempt_count = 0; // prevent loop for going forever
while (attempt_count < MAX_ATTEMPTS)
{
// Generate random state
state.setToRandomPositions(jmg);
state.update(); // prevent dirty transforms
// Test for collision
if (planning_display_->getPlanningSceneRO()->isStateValid(state, "", false))
break;
attempt_count ++;
}
// Explain if no valid rand state found
if (attempt_count >= MAX_ATTEMPTS)
ROS_WARN("Unable to find a random collision free configuration after %d attempts", MAX_ATTEMPTS);
}
else
{
ROS_WARN_STREAM("Unable to get joint model group " << planning_display_->getCurrentPlanningGroup());
}
}
else
if (v == "<current>")
{
const planning_scene_monitor::LockedPlanningSceneRO &ps = planning_display_->getPlanningSceneRO();
if (ps)
state = ps->getCurrentState();
}
else
if (v == "<same as goal>")
{
state = *planning_display_->getQueryGoalState();
}
else
if (v == "<same as start>")
{
state = *planning_display_->getQueryStartState();
}
else
{
// maybe it is a named state
if (const robot_model::JointModelGroup *jmg = state.getJointModelGroup(planning_display_->getCurrentPlanningGroup()))
state.setToDefaultValues(jmg, v);
}
}
