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;
}
int main(int argc, char *argv[])
{
ros::init (argc, argv, "kinematic_model_tutorial");
ros::NodeHandle nh;
ros::AsyncSpinner spinner(1);
spinner.start();
/* Load the robot model */
robot_model_loader::RobotModelLoader robot_model_loader("robot_description");
/* Get a shared pointer to the model */
kinematic_model = robot_model_loader.getModel();
/* Get and print the name of the coordinate frame in
* which the transforms for this model are computed*/
ROS_INFO("Model frame: %s", kinematic_model->getModelFrame().c_str());
/* Create a kinematic state - this represents the configuration for
* the robot represented by kinematic_model */
kinematic_state = robot_state::RobotStatePtr(new robot_state::RobotState(kinematic_model));
/* Set all joints in this state to their default values */
kinematic_state->setToDefaultValues();
joint_model_group = kinematic_model->getJointModelGroup("arm_1");
moveitIKPseudoInverseSolver();
ROS_INFO_STREAM("Test complete.");
return -1;
}
void get_joint_values()
{
kinematic_state->copyJointGroupPositions(joint_model_group, joint_values);
std::cout << "current joints:";
for(int i = 0;i < 5;i++)
std::cout << joint_values[i] << " ";
std::cout << std::endl;
}
// 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;
}
}
void moveit_ik(const Eigen::Affine3d end_effector_state)
{
bool found_ik = kinematic_state->setFromIK(joint_model_group, end_effector_state, 10, 0.1);
if(found_ik)
{
kinematic_state->copyJointGroupPositions(joint_model_group, joint_values);
for(std::size_t i=0; i < 5; ++i)
{
std::cout << "[" << i << "," << joint_values[i] << "],";
}
}
else
{
ROS_INFO("Did not find IK solution.\n");
}
}
Eigen::VectorXd moveit_fk(Eigen::VectorXd jointState)
{
kinematic_state->setJointGroupPositions(joint_model_group, jointState);
const Eigen::Affine3d &end_effector_state = kinematic_state->
getGlobalLinkTransform(kinematic_state->getLinkModel(joint_model_group->getLinkModelNames().back()));
//moveit_ik(end_effector_state);
//Eigen::Quaterniond quaternion (end_effector_state.rotation());
Eigen::VectorXd eulerAngles = end_effector_state.rotation().eulerAngles(0,1,2);
Eigen::VectorXd eefPose(6);
//eefPose << end_effector_state.translation(), quaternion.w(), quaternion.x(), quaternion.y(), quaternion.z();
eefPose << end_effector_state.translation(), eulerAngles;
//ROS_INFO_STREAM("end_eff:\n " << eefPose);
return eefPose;
}
Eigen::MatrixXd moveit_jacobian_inv(Eigen::VectorXd jointState)
{
kinematic_state->setJointGroupPositions(joint_model_group, jointState);
Eigen::Vector3d reference_point_position(0.0,0.0,0.0);
kinematic_state->getJacobian(joint_model_group, kinematic_state->getLinkModel(joint_model_group->getLinkModelNames().back()),
reference_point_position,jacobian);
Eigen::MatrixXd matrix_temp = jacobian * jacobian.transpose();
if(matrix_temp.determinant() != 0)
{
jacPseudoInv = jacobian.transpose() * ((matrix_temp).inverse());
//std::cout << "\n===============================" << endl;
//std::cout << "jacobian:\n " << jacobian << std::endl;
//std::cout << "jacPseudoInv:\n " << jacPseudoInv << std::endl;
//std::cout << "\n===============================" << endl;
//computePivot();
//computerQR();
}
return jacPseudoInv;
}
void NewVizManager::animatePath(const ItompTrajectoryConstPtr& trajectory,
const robot_state::RobotStatePtr& robot_state, bool is_best)
{
if (!is_best)
return;
// marker array
visualization_msgs::MarkerArray ma;
std::vector<std::string> link_names = robot_model_->getMoveitRobotModel()->getLinkModelNames();
std_msgs::ColorRGBA color = colors_[WHITE];
color.a = 1.0;
ros::Duration dur(3600.0);
for (unsigned int point = 0; point < trajectory->getNumPoints(); ++point)
{
ma.markers.clear();
const Eigen::MatrixXd mat = trajectory->getElementTrajectory(
ItompTrajectory::COMPONENT_TYPE_POSITION,
ItompTrajectory::SUB_COMPONENT_TYPE_JOINT)->getTrajectoryPoint(point);
robot_state->setVariablePositions(mat.data());
std::string ns = "frame_" + boost::lexical_cast<std::string>(point);
robot_state->getRobotMarkers(ma, link_names, color, ns, dur);
for (int i = 0; i < ma.markers.size(); ++i)
ma.markers[i].mesh_use_embedded_materials = true;
vis_marker_array_publisher_path_.publish(ma);
}
// MotionPlanning -> Planned Path -> trajectory
moveit_msgs::DisplayTrajectory display_trajectory;
int num_all_joints = robot_state->getVariableCount();
ElementTrajectoryConstPtr joint_trajectory = trajectory->getElementTrajectory(ItompTrajectory::COMPONENT_TYPE_POSITION,
ItompTrajectory::SUB_COMPONENT_TYPE_JOINT);
robot_trajectory::RobotTrajectoryPtr response_trajectory = boost::make_shared<robot_trajectory::RobotTrajectory>(robot_model_->getMoveitRobotModel(), "");
robot_state::RobotState ks = *robot_state;
std::vector<double> positions(num_all_joints);
double dt = trajectory->getDiscretization();
// TODO:
int num_return_points = joint_trajectory->getNumPoints();
for (std::size_t i = 0; i < num_return_points; ++i)
{
for (std::size_t j = 0; j < num_all_joints; j++)
{
positions[j] = (*joint_trajectory)(i, j);
}
ks.setVariablePositions(&positions[0]);
// TODO: copy vel/acc
ks.update();
response_trajectory->addSuffixWayPoint(ks, dt);
}
moveit_msgs::RobotTrajectory trajectory_msg;
response_trajectory->getRobotTrajectoryMsg(trajectory_msg);
display_trajectory.trajectory.push_back(trajectory_msg);
ros::NodeHandle node_handle;
static ros::Publisher display_publisher = node_handle.advertise<moveit_msgs::DisplayTrajectory>("/itomp_planner/display_planned_path", 1, true);
display_publisher.publish(display_trajectory);
}
