Matlab interface

To control clopema robot from matlab, the ROS-Matlab interface will be used. This interface allow us to publish/subscribe topics, calling/providing service and sending goals to actionlib server. The main disadvantages is that using matlab is slower (because of implementation of ROS-Matlab interface). The all msg/srv in this interface are represented as a matlab struct.

How to start ROS-Matlab interface

roslaunch clopema_matlab basic_matlab

Add ROS-Matlab interface functions to matlab (set path)

%% Init (addpath)
clc;clear all;close all;
[voopRes voopT] = system('echo $ROS_WORKSPACE');
addpath(genpath([strcat(voopT) '/clopema_matlab/matlab/']));
clear voopRes voopT

Get Robot Position

There are two ways how to read actual robot position: subscribing to a topic or calling service. Calling service is simple:

robotState = rosClopemaCallService('/environment_server/get_robot_state',struct(''));
if ~isstruct(robotState)
    fprintf('Can''t read robot state.\n');
    return
end

Get robot status (is robot ready)

rdy = rosClopemaCallService('/joint_trajectory_action/is_robot_ready',struct(''))

Planning trajectory

There is a lot of way how to specify goal (see Trajectory planning). All settings available in c++ are also available in Matlab. If planning was success (error_code.val = 1) trajectory is stored in res.joint_trajectory variable.

Joint goal specification

%% specify joint constraints
req = rosClopemaCreateSrv('clopema_arm_navigation/ClopemaMotionPlan');
req.motion_plan_req.allowed_planning_time.secs = 5;
req.motion_plan_req.group_name = 'r1_arm';

robotState = rosClopemaCallService('/environment_server/get_robot_state',struct(''));
if ~isstruct(robotState)
    fprintf('Can''t read robot state.\n');
    return
end
req.motion_plan_req.start_state = robotState.robot_state;

joi = rosClopemaCreateMsg('arm_navigation_msgs/JointConstraint');
joint_names = {'r1_joint_s' 'r1_joint_l' 'r1_joint_u' 'r1_joint_r' 'r1_joint_b' 'r1_joint_t'};
for i = 1:length(joint_names)   
    req.motion_plan_req.goal_constraints.joint_constraints{i}.tolerance_above = 0.05;
    req.motion_plan_req.goal_constraints.joint_constraints{i}.tolerance_below = 0.05;
    req.motion_plan_req.goal_constraints.joint_constraints{i}.weight = 1.0;
    req.motion_plan_req.goal_constraints.joint_constraints{i}.position = 0.5;
    req.motion_plan_req.goal_constraints.joint_constraints{i}.joint_name = joint_names{i};
end

res = rosClopemaCallService('clopema_planner/plan',req);
fprintf('Trajectory planned error code: %d\n', res.error_code.val);

Pose goal specification

clear req;
req.motion_plan_req.allowed_planning_time.secs = 5;
req.motion_plan_req.group_name = 'r1_arm';
robotState = rosClopemaCallService('/environment_server/get_robot_state',struct(''));
if ~isstruct(robotState)
    fprintf('Can''t read robot state.\n');
    return
end
req.motion_plan_req.start_state = robotState.robot_state;
% Specify pose position and orientation
pos.header.frame_id = 'base_link';
pos.link_name = 'r1_tip_link';
pos.constraint_region_orientation.x = 0;
pos.constraint_region_orientation.y = 0;
pos.constraint_region_orientation.z = 0;
pos.constraint_region_orientation.w = 1;
pos.constraint_region_shape.type = 1; %box shape
pos.constraint_region_shape.dimensions{1} = 0.05;
pos.constraint_region_shape.dimensions{2} = 0.05;
pos.constraint_region_shape.dimensions{3} = 0.05;
pos.weight = 1.0;
pos.position.x = -0.5;
pos.position.y = -1.0;
pos.position.z = 0.8;

ori.header.frame_id = 'base_link';
ori.link_name = 'r1_tip_link';
ori.absolute_pitch_tolerance = 0.05;
ori.absolute_roll_tolerance = 0.05;
ori.absolute_yaw_tolerance = 0.05;
ori.weight = 1.0;
ori.orientation.x = 1;
ori.orientation.y = 0;
ori.orientation.z = 0;
ori.orientation.w = 0;

req.motion_plan_req.goal_constraints.position_constraints{1} = pos;
req.motion_plan_req.goal_constraints.orientation_constraints{1} = ori;

res = rosClopemaCallService('clopema_planner/plan',req);
fprintf('Trajectory planned error code: %d\n', res.error_code.val);

Executing trajectory (sending goal to the actionlib server)

%% Execute trajectory
goal = rosClopemaCreateMsg('control_msgs/FollowJointTrajectoryGoal');
fprintf('Sending goal\n');
goal.trajectory = res.joint_trajectory;
result = rosClopemaSendGoal('control_msgs/FollowJointTrajectory', '/clopema_controller/follow_joint_trajectory', goal, 30.0)

Example (go to the home position)

%% Init (addpath)
clc;clear all;close all;
[voopRes voopT] = system('echo $ROS_WORKSPACE');
addpath(genpath([strcat(voopT) '/clopema_matlab/matlab/']));
clear voopRes voopT

%% specify joint constraints
req = rosClopemaCreateSrv('clopema_arm_navigation/ClopemaMotionPlan');
req.motion_plan_req.allowed_planning_time.secs = 5;
req.motion_plan_req.group_name = 'r1_arm';

robotState = rosClopemaCallService('/environment_server/get_robot_state',struct(''));
if ~isstruct(robotState)
    fprintf('Can''t read robot state.\n');
    return
end
req.motion_plan_req.start_state = robotState.robot_state;

joint_names = {'r1_joint_s' 'r1_joint_l' 'r1_joint_u' 'r1_joint_r' 'r1_joint_b' 'r1_joint_t'};
for i = 1:length(joint_names)   
    req.motion_plan_req.goal_constraints.joint_constraints{i}.tolerance_above = 0.05;
    req.motion_plan_req.goal_constraints.joint_constraints{i}.tolerance_below = 0.05;
    req.motion_plan_req.goal_constraints.joint_constraints{i}.weight = 1.0;
    req.motion_plan_req.goal_constraints.joint_constraints{i}.position = 0.0;
    req.motion_plan_req.goal_constraints.joint_constraints{i}.joint_name = joint_names{i};
end

res = rosClopemaCallService('clopema_planner/plan',req);
fprintf('Trajectory planned error code: %d\n', res.error_code.val);

%% Execute trajectory
goal = rosClopemaCreateMsg('control_msgs/FollowJointTrajectoryGoal');
fprintf('Sending goal\n');
goal.trajectory = res.joint_trajectory;
result = rosClopemaSendGoal('control_msgs/FollowJointTrajectory', '/clopema_controller/follow_joint_trajectory', goal, 30.0)