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();
}
bool setRandomValidState(robot_state::RobotStatePtr &state, const robot_model::JointModelGroup* jmg)
{
// 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 staten
state->setToRandomPositions(jmg);
state->update(); // prevent dirty transforms
// Test for collision
if (planning_scene_->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);
return false;
}
return true;
}
// 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;
}
}