コード例 #1
0
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();
}
コード例 #2
0
  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;
  }
コード例 #3
0
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;
}
コード例 #4
0
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;
}
コード例 #5
0
// 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;
    }
}
コード例 #6
0
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");
	}
}
コード例 #7
0
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;
}
コード例 #8
0
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;
}
コード例 #9
0
ファイル: new_viz_manager.cpp プロジェクト: Chpark/itomp
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);
}