void GeometricFixedPosePlanningContext::shiftStateByError(robot_state::RobotStatePtr state, const robot_state::JointModel* base_joint,
const std::string& link, const Eigen::Affine3d& desired_pose) const
{
if (base_joint->getType() != robot_state::JointModel::FLOATING)
{
ROS_ERROR("%s: Expected root joint '%s' to be floating. This joint is %s",
__FUNCTION__, base_joint->getName().c_str(), base_joint->getTypeName().c_str());
return;
}
Eigen::Affine3d actual_pose = state->getGlobalLinkTransform(link);
// This is the error in the pose
Eigen::Affine3d diff(desired_pose * actual_pose.inverse());
// Apply the difference to the (unconstrained) base frame
// VERY important that diff is multiplied on left side
Eigen::Affine3d new_base_frame = diff * state->getGlobalLinkTransform(base_joint->getChildLinkModel()->getName());
Eigen::Quaterniond q(new_base_frame.rotation());
// Variable order is hard-coded for efficiency
// This assumes a fixed variable order that "could" change in a future version of MoveIt
std::vector<double> new_variables(7, 0.0);
new_variables[0] = new_base_frame.translation()(0); // base_joint_model->getName() + "/trans_x"
new_variables[1] = new_base_frame.translation()(1); // base_joint_model->getName() + "/trans_y"
new_variables[2] = new_base_frame.translation()(2); // base_joint_model->getName() + "/trans_z"
new_variables[3] = q.x(); // base_joint_model->getName() + "/rot_x"
new_variables[4] = q.y(); // base_joint_model->getName() + "/rot_y"
new_variables[5] = q.z(); // base_joint_model->getName() + "/rot_z"
new_variables[6] = q.w(); // base_joint_model->getName() + "/rot_w"
// Setting new base position
state->setJointPositions(base_joint, new_variables);
state->update();
}
// update the allowed collision matrix to allow/disallow collisions with handrails
void adjustAllowedHandrailCollisions(R2Interface& interface, robot_state::RobotStatePtr currentState, const ISSWorld& world,
bool leftLegFixed, const std::string& handrail)
{
std::string touchingHandrail = getHandrailAtPose(world, currentState->getGlobalLinkTransform(leftLegFixed ? LEFT_GRIPPER_LINK0 : RIGHT_GRIPPER_LINK0));
if (touchingHandrail == "")
touchingHandrail = getHandrailAtPose(world, currentState->getGlobalLinkTransform(leftLegFixed ? LEFT_GRIPPER_LINK1 : RIGHT_GRIPPER_LINK1));
// Disable collisions with the destination handrail
if (touchingHandrail == "")
ROS_ERROR("No handrail collision detected. Expected %s to be touching a handrail.", leftLegFixed ? "left leg" : "right leg");
else
{
letGripperTouchObject(interface, leftLegFixed, touchingHandrail, true); // make the start state valid
letGripperTouchObject(interface, !leftLegFixed, touchingHandrail, true); // make the start state valid
letGripperTouchObject(interface, leftLegFixed ? false : true, handrail, true); // make the goal state valid
std::cout << (leftLegFixed ? "Right " : "Left ") << "leg is allowed to collide with " << handrail << std::endl;
// TODO: Should turn collisions back on for previous handrails
// Make sure the gripper is open for the moving leg and closed for fixed leg
currentState->setVariablePosition((leftLegFixed ? "r2/right_leg/gripper/jawLeft" : "r2/left_leg/gripper/jawLeft"), 0.75);
currentState->setVariablePosition((leftLegFixed ? "r2/right_leg/gripper/jawRight" : "r2/left_leg/gripper/jawRight"), 0.75);
currentState->setVariablePosition((leftLegFixed ? "r2/left_leg/gripper/jawLeft" : "r2/right_leg/gripper/jawLeft"), 0.0);
currentState->setVariablePosition((leftLegFixed ? "r2/left_leg/gripper/jawRight" : "r2/right_leg/gripper/jawRight"), 0.0);
currentState->update();
std::cout << (leftLegFixed ? "Left " : "Right ") << "leg is holding " << touchingHandrail << std::endl;
}
}