ROSAction-MATLAB 联调 Control PR2 Arm运动操作

转载自maltlab官网

https://ww2.mathworks.cn/help/robotics/examples/control-pr2-arm-movements-using-actions-and-ik.html

Control PR2 Arm Movements Using ROS Actions and Inverse Kinematics

 

友情提示：

如果maltab安装在windows端，ROS安装在虚拟机端，则一定要将windows端的防火墙关闭，否则在waitForServer(rArm)时maltab会一直卡住，请注意

Try it in MATLAB

This example shows how to send commands to robotic manipulators in MATLAB®. Joint position commands are sent via a ROS action client over a ROS network. This example also shows how to calculate joint positions for a desired end-effector position. A rigid body tree defines the robot geometry and joint constraints, which is used with inverse kinematics to get the robot joint positions. You can then send these joint positions as a trajectory to command the robot to move.

Bring up PR2 Gazebo Simulation

Spawn PR2 in a simple environment (only with a table and a coke can) in Gazebo Simulator. Follow steps in the Get Started with Gazebo and a Simulated TurtleBot to launch the Gazebo PR2 Simulator from the Ubuntu® virtual machine desktop.

 

 

Connect to ROS Network from MATLAB®

In your MATLAB instance on the host computer, run the following commands to initialize ROS global node in MATLAB and connect to the ROS master in the virtual machine through its IP address ipaddress. Replace '192.168.245.130' with the IP address of your virtual machine.

ipaddress = '192.168.203.129';
rosinit(ipaddress);

Initializing global node /matlab_global_node_05090 with NodeURI http://192.168.203.1:55362/

Create Action Clients for Robot Arms and Send Commands

In this task, you send joint trajectories to the PR2 arms. The arms can be controlled via ROS actions. Joint trajectories are manually specified for each arm and sent as separate goal messages via separate action clients.

Create a ROS simple action client to send goal messages to move the right arm of the robot. rosactionclient object and goal message are returned. Wait for the client to connect to the ROS action server.

[rArm, rGoalMsg] = rosactionclient('r_arm_controller/joint_trajectory_action');
waitForServer(rArm);

The goal message is a trajectory_msgs/JointTrajectoryPoint message. Each trajectory point should specify positions and velocities of the joints.

disp(rGoalMsg)

  ROS JointTrajectoryGoal message with properties:

    MessageType: 'pr2_controllers_msgs/JointTrajectoryGoal'
     Trajectory: [1×1 JointTrajectory]

  Use showdetails to show the contents of the message

disp(rGoalMsg.Trajectory)

  ROS JointTrajectory message with properties:

    MessageType: 'trajectory_msgs/JointTrajectory'
         Header: [1×1 Header]
     JointNames: {0×1 cell}
         Points: [0×1 JointTrajectoryPoint]

  Use showdetails to show the contents of the message

Set the joint names to match the PR2 robot joint names. Note that there are 7 joints to control. To find what joints are in PR2 right arm, type this command in the virtual machine terminal:

rosparam get /r_arm_controller/joints 

rGoalMsg.Trajectory.JointNames = {'r_shoulder_pan_joint', ...
                                   'r_shoulder_lift_joint', ...
                                   'r_upper_arm_roll_joint', ...
                                   'r_elbow_flex_joint',...
                                   'r_forearm_roll_joint',...
                                   'r_wrist_flex_joint',...
                                   'r_wrist_roll_joint'};

Create setpoints in the arm joint trajectory by creating ROS trajectory_msgs/JointTrajectoryPoint messages and specifying the position and velocity of all 7 joints as a vector. Specify a time from the start as a ROS duration object.

% Point 1
tjPoint1 = rosmessage('trajectory_msgs/JointTrajectoryPoint');
tjPoint1.Positions = zeros(1,7);
tjPoint1.Velocities = zeros(1,7);
tjPoint1.TimeFromStart = rosduration(1.0);

% Point 2
tjPoint2 = rosmessage('trajectory_msgs/JointTrajectoryPoint');
tjPoint2.Positions = [-1.0 0.2 0.1 -1.2 -1.5 -0.3 -0.5];
tjPoint2.Velocities = zeros(1,7);
tjPoint2.TimeFromStart = rosduration(2.0);

Create an object array with the points in the trajectory and assign it to the goal message. Send the goal using the action client. The sendGoalAndWait function will block execution until the PR2 arm finishes executing the trajectory

rGoalMsg.Trajectory.Points = [tjPoint1,tjPoint2];

sendGoalAndWait(rArm,rGoalMsg);

 

 

Create another action client to send commands to the left arm. Specify the joint names of the PR2 robot.

[lArm, lGoalMsg] = rosactionclient('l_arm_controller/joint_trajectory_action');
waitForServer(lArm);

lGoalMsg.Trajectory.JointNames = {'l_shoulder_pan_joint', ...
                                   'l_shoulder_lift_joint', ...
                                   'l_upper_arm_roll_joint', ...
                                   'l_elbow_flex_joint',...
                                   'l_forearm_roll_joint',...
                                   'l_wrist_flex_joint',...
                                   'l_wrist_roll_joint'};

Set a trajectory point for the left arm. Assign it to the goal message and send the goal.

tjPoint3 = rosmessage('trajectory_msgs/JointTrajectoryPoint');
tjPoint3.Positions = [1.0 0.2 -0.1 -1.2 1.5 -0.3 0.5];
tjPoint3.Velocities = zeros(1,7);
tjPoint3.TimeFromStart = rosduration(2.0);

lGoalMsg.Trajectory.Points = tjPoint3;

sendGoalAndWait(lArm,lGoalMsg);

 

 

Calculate Inverse Kinematics for an End-Effector Position

In this section, you calculate a trajectory for joints based on the desired end-effector (PR2 right gripper) positions. The robotics.InverseKinematics> class calculates all the required joint positions, which can be sent as a trajectory goal message via the action client. A robotics.RigidBodyTree class is used to define the robot parameters, generate configurations, and visualize the robot in MATLAB®.

Perform The following steps:

• Get the current state of the PR2 robot from the ROS network and assign it to a robotics.RigidBodyTree object to work with the robot in MATLAB®.

• Define an end-effector goal pose.

• Visualize the robot configuration using these joint positions to ensure a proper solution.

• Use inverse kinematics to calculate joint positions from goal end-effector poses.

• Send the trajectory of joint positions to the ROS action server to command the actual PR2 robot.

Create a Rigid Body Tree in MATLAB®

Load a PR2 robot as a robotics.RigidBodyTree object. This object defines all the kinematic parameters (including joint limits) of the robot.

pr2 = exampleHelperWGPR2Kinect;

Get the Current Robot State

Create a subscriber to get joint states from the robot.

jointSub = rossubscriber('joint_states');

Get the current joint state message.

jntState = receive(jointSub);

Assign the joint positions from the joint states message to the fields of a configuration struct that the pr2 object understands.

jntPos = exampleHelperJointMsgToStruct(pr2,jntState);

Visualize the Current Robot Configuration

Use show to visualize the robot with the given joint position vector. This should match the current state of the robot.

show(pr2,jntPos)

ans = 
  Axes (Primary) with properties:

             XLim: [-2 2]
             YLim: [-2 2]
           XScale: 'linear'
           YScale: 'linear'
    GridLineStyle: '-'
         Position: [0.1300 0.1100 0.7750 0.8150]
            Units: 'normalized'

  Show all properties

Create an robotics.InverseKinematics object from the pr2 robot object. The goal of inverse kinematics is to calculate the joint angles for the PR2 arm that places the gripper (i.e. the end-effector) in a desired pose. A sequence of end-effector poses over a period of time is called a trajectory.

In this example, we will only be controlling the robot's arms. Therefore, we place tight limits on the torso-lift joint during planning.

torsoJoint = pr2.getBody('torso_lift_link').Joint;
idx = strcmp({jntPos.JointName}, torsoJoint.Name);
torsoJoint.HomePosition = jntPos(idx).JointPosition;
torsoJoint.PositionLimits = jntPos(idx).JointPosition + [-1e-3,1e-3];

Create the InverseKinematics object.

ik = robotics.InverseKinematics('RigidBodyTree', pr2);

Disable random restart to ensure consistent IK solutions.

ik.SolverParameters.AllowRandomRestart = false;

Specify weights for the tolerances on each component of the goal pose.

weights = [0.25 0.25 0.25 1 1 1];
initialGuess = jntPos; % current jnt pos as initial guess

Specify end-effector poses related to the task. In this example, PR2 will reach to the can on the table, grasp the can, move it to a different location and set it down again. We will use the InverseKinematics object to plan motions that smoothly transition from one end-effector pose to another.

Specify the name of the end effector.

endEffectorName = 'r_gripper_tool_frame';

Specify the can's initial (current) pose and the desired final pose.

TCanInitial = trvec2tform([0.7, 0.0, 0.55]);
TCanFinal = trvec2tform([0.6, -0.5, 0.55]);

Define the desired relative transform between the end-effector and the can when grasping.

TGraspToCan = trvec2tform([0,0,0.08])*eul2tform([pi/8,0,-pi]);

Define the key waypoints of a desired Cartesian trajectory. The can should move along this trajectory. The trajectory can be broken up as follows:

• Open the gripper

• Move the end-effector from current pose to T1 so that the robot will feel comfortable to initiate the grasp

• Move the end-effector from T1 to TGrasp (ready to grasp)

• Close the gripper and grab the can

• Move the end-effector from TGrasp to T2 (lift can in the air)

• Move the end-effector from T2 to T3 (move can levelly)

• Move the end-effector from T3 to TRelease (lower can to table surface and ready to release)

• Open the gripper and let go of the can

• Move the end-effector from TRelease to T4 (retract arm)

TGrasp = TCanInitial*TGraspToCan; % The desired end-effector pose when grasping the can
T1 = TGrasp*trvec2tform([0.,0,-0.1]);
T2 = TGrasp*trvec2tform([0,0,-0.2]);
T3 = TCanFinal*TGraspToCan*trvec2tform([0,0,-0.2]);
TRelease = TCanFinal*TGraspToCan; % The desired end-effector pose when releasing the can
T4 = T3*trvec2tform([-0.1,0,0]);

The actual motion will be specified by numWaypoints joint positions in a sequence and sent to the robot through the ROS simple action client. These configurations will be chosen using the InverseKinematics object such that the end effector pose is interpolated from the initial pose to the final pose.

Execute the Motion

Specify the sequence of motions.

motionTask = {'Release', T1, TGrasp, 'Grasp', T2, T3, TRelease, 'Release', T4};

Execute each task specified in motionTask one by one. Send commands using the specified helper functions.

for i = 1: length(motionTask)
    
    if strcmp(motionTask{i}, 'Grasp')
        exampleHelperSendPR2GripperCommand('right',0.0,1000,true); 
        continue
    end
    
    if strcmp(motionTask{i}, 'Release')
        exampleHelperSendPR2GripperCommand('right',0.1,-1,true);
        continue
    end  
    
    Tf = motionTask{i};
    % Get current joint state
    jntState = receive(jointSub);
    jntPos = exampleHelperJointMsgToStruct(pr2, jntState);
    
    T0 = getTransform(pr2, jntPos, endEffectorName);  
    
    % Interpolating between key waypoints
    numWaypoints = 10;
    TWaypoints = exampleHelperSE3Trajectory(T0, Tf, numWaypoints); % end-effector pose waypoints
    jntPosWaypoints = repmat(initialGuess, numWaypoints, 1); % joint position waypoints
    
    rArmJointNames = rGoalMsg.Trajectory.JointNames;
    rArmJntPosWaypoints = zeros(numWaypoints, numel(rArmJointNames));
    
    % Calculate joint position for each end-effector pose waypoint using IK
    for k = 1:numWaypoints
        jntPos = ik(endEffectorName, TWaypoints(:,:,k), weights, initialGuess);
        jntPosWaypoints(k, :) = jntPos;
        initialGuess = jntPos;
        
        % Extract right arm joint positions from jntPos
        rArmJointPos = zeros(size(rArmJointNames));
        for n = 1:length(rArmJointNames)
            rn = rArmJointNames{n};
            idx = strcmp({jntPos.JointName}, rn);
            rArmJointPos(n) = jntPos(idx).JointPosition;
        end  
        rArmJntPosWaypoints(k,:) = rArmJointPos'; 
    end
    
    % Time points corresponding to each waypoint
    timePoints = linspace(0,3,numWaypoints);
        
    % Estimate joint velocity trajectory numerically
    h = diff(timePoints); h = h(1);
    jntTrajectoryPoints = repmat(rosmessage('trajectory_msgs/JointTrajectoryPoint'),1,numWaypoints);
    [~, rArmJntVelWaypoints] = gradient(rArmJntPosWaypoints, h);
    for m = 1:numWaypoints
        jntTrajectoryPoints(m).Positions = rArmJntPosWaypoints(m,:);
        jntTrajectoryPoints(m).Velocities = rArmJntVelWaypoints(m,:);
        jntTrajectoryPoints(m).TimeFromStart = rosduration(timePoints(m));
    end
    
    % Visualize robot motion and end-effector trajectory in MATLAB(R)
    hold on
    for j = 1:numWaypoints
        show(pr2, jntPosWaypoints(j,:),'PreservePlot', false);
        exampleHelperShowEndEffectorPos(TWaypoints(:,:,j));
        drawnow;
        pause(0.1);
    end
    
    % Send the right arm trajectory to the robot
    rGoalMsg.Trajectory.Points = jntTrajectoryPoints;
    sendGoalAndWait(rArm, rGoalMsg);

end

 

 

 

 

Wrap Up

Move arm back to starting position.

exampleHelperSendPR2GripperCommand('r',0.0,-1)
rGoalMsg.Trajectory.Points = tjPoint2;
sendGoal(rArm, rGoalMsg);

Call rosshutdown to shutdown ROS network and disconnect from the robot.

rosshutdown

Shutting down global node /matlab_global_node_05090 with NodeURI http://192.168.203.1:55362/