Difference between revisions of "Arm Control Systems"

Jump to: navigation , search
m (Motion Controller Interface)
(Joint Control Fundamentals)
 
(33 intermediate revisions by 3 users not shown)
Line 1: Line 1:
{{TOClimit|limit=3}}
+
{{TOClimit|limit=2}}
  
 
<div class="content-block">
 
<div class="content-block">
Line 5: Line 5:
 
= Control Overview =
 
= Control Overview =
  
There are two primary workflows for moving the arm via the SDK. The first workflow sends high frequency joint commands to the realtime motor controller via one of four control modes. New in Intera 5.2, the second workflow sends trajectory commands to the Motion Controller which generate and execute a smooth trajectory between waypoints.
+
There are two primary workflows for moving the arm via the SDK. The first workflow sends high frequency joint commands to the realtime motor controller via one of four control modes.  
 +
The second workflow is the [[#Motion Controller Interface | motion interface]], introduced in the Intera 5.2 software release, which sends trajectory planning request to the Motion Controller to generate and execute a smooth trajectory between waypoints.
  
On top of two ways of moving the arm, [[#INTERACTION CONTROL MODE | interaction control mode]] can implement either an impedance controller or force controller about the endpoint.
+
An additional new feature in Intera 5.2 is the [[#Interaction Control Mode | interaction control mode]], which can implement either an impedance controller or force controller at the endpoint of the arm. Interaction control mode can be used by itself, or used in conjunction with the motion interface to generate compliant behavior along a trajectory.
  
  
Line 27: Line 28:
  
  
== Motion Controller Interface ==
+
== Motion Controller Interface Overview ==
 
'''New in Intera 5.2!'''
 
'''New in Intera 5.2!'''
  
 
[[File:SDK_Interface_Drawing_Workflow_2.png|420px|thumb|left]]
 
[[File:SDK_Interface_Drawing_Workflow_2.png|420px|thumb|left]]
  
Users who want basic motions without generating their own trajectories should use the Motion Controller interface. The motion controller interface is a ROS action server that allows the SDK access to built-in trajectory generation and motion control features between gross waypoints. Smooth trajectories can be created as either joint splines or Cartesian paths, and interaction control parameters can be included with the trajectory options. Note that the Motion Controller does not know about external obstacles (or end effector collision shapes) and will fail before causing a self collision.
+
Users who want basic motions without generating their own trajectories should use the Motion Controller interface. The motion controller interface is a ROS action server that allows the SDK access to built-in trajectory generation and motion control features between widely spaced waypoints. Smooth trajectories can be created in either joint mode (move quickly between waypoints) or cartesian mode (end-effector moves along straight lines). Interaction control parameters can be included with the options for each waypoint to allow for compliant behavior along a trajectory. Note that the Motion Controller will not allow trajectories that result in self collision. However, it does not know about end-effector collision shapes or obstacles in the environment, so these are not considered when running the collision check.
 +
 
  
 
== RealTime Motor Controller ==
 
== RealTime Motor Controller ==
The RealTime Motor Control loop on the robot executes over the course of <code>1ms</code>, and approximately <code>700us</code> of this loop is used on average. Occasionally you may find an overrun of the RealTime loop around <code>100us</code>. This overrun usually does not propagate forward to other cycles because the next loop will likely take <code>700us</code>. To verify this you can look at the diagnostics messages via "rostopic echo /diagnostics", and look for the "Realtime Control Loop".
+
The RealTime Motor Control loop runs on the robot and executes over the course of <code>1ms</code>, and approximately <code>700us</code> of this loop is used on average. Occasionally you may find an overrun of the RealTime loop around <code>100us</code>. This overrun usually does not propagate forward to other cycles because the next loop will likely take <code>700us</code>. To verify this you can look at the diagnostics messages via "rostopic echo /diagnostics", and look for the "Realtime Control Loop".
  
The RealTime Motor Controller has plugins that run simultaneously with joint commands. These are Collision Avoidance, Collision Detection and Gravity Compensation. On top of these plugins, when Sawyer's cuff pressure sensor is activated, Zero-G Mode enables, allowing the arm to be passively back driven. Upon releasing the cuff, the motor controller returns to the controller it was using previously.
+
The RealTime Motor Controller has plugins that run simultaneously with joint commands. These are Collision Avoidance, Collision Detection and Gravity Compensation. On top of these plugins, when Sawyer's cuff pressure sensor is activated, Zero-G Mode enables, allowing the arm to be passively back driven. Upon releasing the cuff, the motor controller returns to the controller it was using previously. For more detailed log information, either view the log messages in the [http://wiki.ros.org/rqt_console rqt_console], by filtering for the <code>realtime_loop</code> node, or see the <code>realtime_loop-stdout.log</code> export for more details.
  
 
</div>
 
</div>
Line 94: Line 96:
 
== Joint Control Fundamentals ==
 
== Joint Control Fundamentals ==
  
The '<code>realtime_loop</code>' process subscribes to joitn position commands published from the development workstation. Modifications, based on control mode, can be made to the input commands. These modifications are typically due to the safety controllers (e.g. arm-to-arm collision avoidance, collision detection, etc.)
+
The '<code>realtime_loop</code>' process subscribes to joint position commands published from the development workstation. Modifications, based on control mode, can be made to the input commands. These modifications are typically due to the safety controllers (e.g. arm-to-arm collision avoidance, collision detection, etc.)
  
 
A control rate timeout is also enforced at this motor controller layer. If a new '<code>JointCommand</code>' message is not received within the specified timeout (default of 0.2 seconds, or 5Hz), the robot will 'Timeout'. When the robot 'Times out', the current control mode is exited, reverting back to position control mode where the robot will command (hold) it's current joint angles. This timeout can prevent dangerous motions due to network issues (e.g. someone kicks your router).
 
A control rate timeout is also enforced at this motor controller layer. If a new '<code>JointCommand</code>' message is not received within the specified timeout (default of 0.2 seconds, or 5Hz), the robot will 'Timeout'. When the robot 'Times out', the current control mode is exited, reverting back to position control mode where the robot will command (hold) it's current joint angles. This timeout can prevent dangerous motions due to network issues (e.g. someone kicks your router).
Line 103: Line 105:
 
</syntaxhighlight>
 
</syntaxhighlight>
 
of message type [http://docs.ros.org/api/std_msgs/html/msg/Float64.html std_msgs/Float64]
 
of message type [http://docs.ros.org/api/std_msgs/html/msg/Float64.html std_msgs/Float64]
 
  
 
== Joint Position Control ==
 
== Joint Position Control ==
'''In Intera 5.2 and beyond, the Motion Controller Interface is recommend instead of Joint Position Control for smoother arm motion.'''
+
'''Depreciated for Intera 5.2 and beyond: use the Motion Controller Interface instead.'''
  
Joint position control mode is the fundamental, basic control mode for Sawyer arm motion. In position control mode, we specify joint angles at which we want the joints to achieve. Typically this will be consist of seven values, a commanded position for each of the seven joints, resulting in a full description of the arm configuration.
+
Joint position control mode is a simple control mode for Sawyer arm motion. In position control mode, we specify joint angles at which we want the joints to achieve. Typically this will be consist of seven values, a commanded position for each of the seven joints, resulting in a full description of the arm configuration. This control mode can result in jerky-motions and should generally be avoided.
  
 
JointCommand.msg mode: <code>POSITION_MODE</code>
 
JointCommand.msg mode: <code>POSITION_MODE</code>
Line 115: Line 116:
  
 
[[File:Joint_Position_Control.png|550px]]
 
[[File:Joint_Position_Control.png|550px]]
 
  
 
== Joint Trajectory-Position Control ==
 
== Joint Trajectory-Position Control ==
  
Joint Trajectory-Position Control mode provides a more direct (time-based) position control method for following planned trajectories, including desired joint velocities and accelerations. The same basic idea as the standard Joint Position Control mode still holds. However, the joint commands are largely left to the JCBs for execution.
+
Joint Trajectory-Position Control mode provides the most direct way for the robot to follow a time-based reference trajectory.
 +
The user must supply position, velocity, and acceleration in each command, which results in a far smoother motion than the Joint Position Control mode.  
 +
The Realtime loop will attempt to move to the desired joint angles position, with the desired joint velocity.
 +
The joint accelerations (along with commanded position and velocity) are used to extrapolate between successive commands and to generate feed-forward torque terms to improve the dynamic trajectory tracking performance.
  
 
JointCommand.msg mode: <code>TRAJECTORY_MODE</code>
 
JointCommand.msg mode: <code>TRAJECTORY_MODE</code>
Line 127: Line 130:
 
[[File:'Raw'_Joint_Position_Control.png|550px]]
 
[[File:'Raw'_Joint_Position_Control.png|550px]]
  
'''Warning:''' Advanced Control Mode. As we no longer apply Collision Avoidance offsets, and allow for very fast motions at the joints velocity limits, this mode can be dangerous. Please use with caution.
+
'''Warning:''' Advanced Control Mode. As this mode does not have Collision Avoidance offsets, and allows for very fast motions at the joints velocity limits, this mode can be dangerous. Please use with caution.
 
 
  
 
== Joint Velocity Control ==
 
== Joint Velocity Control ==
Line 178: Line 180:
  
 
= Motion Controller Interface =
 
= Motion Controller Interface =
'''New in Intera 5.2!'''
 
  
The motion controller interface allows the SDK to access the built-in trajectory generation and motion control features in Sawyer. It provides a set of classes that are used to construct an <code>intera_motion_msgs/Trajectory</code> message, as well as an action server that will pass that message along to the motion controller. The motion controller will then generate and run a motion plan from that trajectory message.
+
The motion controller interface allows the SDK access to built-in trajectory generation and motion control features in Sawyer. The python [https://github.com/RethinkRobotics/intera_sdk/tree/development/intera_interface/src/intera_motion_interface intera motion interface] provides a set of classes used to construct [https://github.com/RethinkRobotics/intera_common/tree/development/intera_motion_msgs intera motion messages] as well as an ROS action client which communicates with the Motion Controller on the robot controller. The Motion Controller will then generate and run a motion plan from that trajectory message.
 +
 
 +
 
 +
== ROS Action Server ==
 +
 
 +
The Motion Controller has a [http://wiki.ros.org/actionlib ROS Action server] on the topic:
 +
<syntaxhighlight lang="bash" enclose="div">
 +
/motion/motion_command
 +
</syntaxhighlight>
 +
 
 +
The goal message has a string command which can be either "start" or "stop" along with a <code>intera_motion_msgs/Trajectory</code> message to define the desired motion. The <code>intera_motion_msgs/MotionStatus</code> provides feedback during the trajectory which includes the <code>motion_status</code> along with the <code>current_trajectory</code> and <code>current_waypoint ids</code>. A result message is returned after either a successful or failed trajectory completion; it includes a boolean <code>result</code> and string <code>errorID</code> if needed. Trajectories will stop if a collision is detected by the robot or the robot is disabled.
 +
 
 +
The Motion Controller will generate a trajectory through all of the waypoints starting at the current joint configuration and stopping at the last waypoint. The planner will avoid stopping at the middle waypoints when possible. If a new trajectory is sent before the current one is finished, then the current motion will stop.
 +
The new trajectory will be generated starting from the current joint positions (where the arm stopped moving).
 +
The trajectory interpolation type can be set to either Joint or Cartesian; mixed trajectory interpolation types are not allowed.
 +
 
 +
Before executing a planned motion, the Motion Controller first sub-samples the path to check for self collisions (not including external obstacles or end effector collision shapes). If a planned collision is found, the Motion Controller will return a PLANNED_MOTION_COLLISION error in the result message. To disable collision checking, publish an empty message at greater than 5 Hz to the topic <code>/robot/limb/right/suppress_collision_avoidance</code>.
 +
 
 +
'''New in Intera 5.3:''' When the motion controller receives a stop command while running a motion it will now perform a smooth path-following stop over 300 milliseconds.
 +
 
 +
== Trajectory Interpolation:  Joint vs. Cartesian ==
 +
 
 +
The motion controller supports two basic methods for interpolation between waypoints: JOINT mode and CARTESIAN mode. Joint mode should generally be preferred to Cartesian mode, as it results in motion that is faster and smoother than Cartesian mode.
 +
 
 +
'''JOINT MODE:''' For trajectories with joint interpolation, the motion controller will generate a fast and smooth path between waypoints. It may or may not stop at intermediate waypoints, and the path is generally non-linear in joint space (unless there is a single waypoint).
 +
 
 +
'''CARTESIAN MODE:''' The motion planning for Cartesian trajectories occurs in two steps:
 +
1) A trajectory is generated in cartesian space that moves linearly (position + orientation) between each waypoint. If the blending radius option is enabled, then the planner will allow these linear paths to be connected with circular arcs and the motion will not stop as it passes near each waypoint.
 +
2) An inverse-kinematics solver is used to compute the joint angles to move the endpoint along the reference trajectory. If joint angle are provided along with the Cartesian poses in the waypoints, the joint angles will be used to bias the IK solutions along each trajectory segment. Note that the active_endpoint must be consistent for all waypoints within a trajectory.
 +
 
 +
== Possible Errors ==
 +
 
 +
Here is a list of possible errors which could be returned from the Motion Controller action server in the result message. For more detailed information from a reported error, either view the log messages in the [http://wiki.ros.org/rqt_console rqt_console], by filtering for the <code>motion_controller</code> node, or see the <code>motion_controller-stdout.log</code> export for more details.
 +
 
 +
; INVALID_TRAJECTORY_MESSAGE : The trajectory message was either missing necessary parameters or the parameters were inconsistent
 +
; FAILED_TO_PARAMETERIZE : A valid joint trajectory spline could not be generated
 +
; PLANNED_MOTION_COLLISION : Prior to moving, a self-collision was found along the planned trajectory
 +
; ENDPOINT_DOES_NOT_EXIST : For a Cartesian trajectory, the endpoint string was not found in the current URDF
 +
; CARTESIAN_INTERPOLATION_FAILED : A valid Cartesian trajectory could not be generate or there was an Inverse Kinemaics solution could not be found
 +
; PLANNED_JOINT_ACCEL_LIMIT : The generated Cartesian trajectory would violate joint limits. Try reducing the Cartesian speeds and accelerations.
 +
; FINAL_POSE_NOT_WITHIN_TOLERANCE : The trajectory finished, but the joints did not settle within the desired joint tolerance
 +
; CONTROLLER_NOT_FOLLOWING : The trajectory was stopped because either the arm detected a collision or the arm was disabled
 +
; ZERO_G_ACTIVATED_DURING_TRAJECTORY : The trajectory was stopped because zero-g was activated mid-motion
  
 
== Trajectory Message ==
 
== Trajectory Message ==
A <code>intera_motion_msgs/Trajectory</code> message is used to specify a trajectory for Sawyer. It contains a sequence of waypoints that the trajectory should pass through, as well as the interpolation type, kinematic constraints, and interaction parameters. Generally the trajectory message is created using the <code>MotionTrajectory</code> class, but advanced users might choose to create it directly. A trajectory message can be sent to the motion controller using either the <code>MotionTrajectory</code> class (prefered method) or the <code>MotionControllerActionClient</code> class (advanced users).
+
A <code>intera_motion_msgs/Trajectory</code> message is used to specify a trajectory for Sawyer. It contains a sequence of waypoints that the trajectory should pass through, as well as the interpolation type, kinematic constraints, and interaction parameters. The Motion Controller always appends the current arm configuration to the beginning of the trajectory. Generally the trajectory message is created using the <code>MotionTrajectory</code> class, but advanced users might choose to create it directly. A trajectory message can be sent to the motion controller using either the <code>MotionTrajectory</code> class (preferred method) or the <code>MotionControllerActionClient</code> class (advanced users).
 +
 
  
 
== Motion Trajectory ==
 
== Motion Trajectory ==
 
The <code>MotionTrajectory</code> class is used as a utility to create and send <code>intera_motion_msgs/Trajectory</code> messages. The simplest way to use it is to call the <code>append_waypoint()</code> to build up a sequence of waypoints and then call <code>send_trajectory()</code> to plan and execute the a trajectory that passes through the waypoints. The <code>MotionWaypoint</code>, <code>MotionWaypointOptions</code>, and <code>InteractionOptions</code> classes are all utilities that help with the construction of trajectories.
 
The <code>MotionTrajectory</code> class is used as a utility to create and send <code>intera_motion_msgs/Trajectory</code> messages. The simplest way to use it is to call the <code>append_waypoint()</code> to build up a sequence of waypoints and then call <code>send_trajectory()</code> to plan and execute the a trajectory that passes through the waypoints. The <code>MotionWaypoint</code>, <code>MotionWaypointOptions</code>, and <code>InteractionOptions</code> classes are all utilities that help with the construction of trajectories.
 +
  
 
== Motion Waypoint ==
 
== Motion Waypoint ==
 
The <code>MotionWaypoint</code> class is used as utility to create <code>intera_motion_msgs/Waypoint</code> messages. It can be directly passed to the <code>append_waypoint()</code> method of a <code>MotionTrajectory</code>, or you can use the <code>to_msg()</code> method to directly use the ROS message format. All unspecified parameters are set to reasonable default values.
 
The <code>MotionWaypoint</code> class is used as utility to create <code>intera_motion_msgs/Waypoint</code> messages. It can be directly passed to the <code>append_waypoint()</code> method of a <code>MotionTrajectory</code>, or you can use the <code>to_msg()</code> method to directly use the ROS message format. All unspecified parameters are set to reasonable default values.
 +
  
 
== Motion Waypoint Options==
 
== Motion Waypoint Options==
 
The <code>MotionWaypointOptions</code> class is used as utility to create <code>intera_motion_msgs/WaypointOptions</code> messages.
 
The <code>MotionWaypointOptions</code> class is used as utility to create <code>intera_motion_msgs/WaypointOptions</code> messages.
 
A <code>MotionWaypointOptions</code> object can be passed to <code>MotionWaypoint.set_waypoint_options()</code>, or you can use the <code>to_msg()</code> method to directly use the ROS message format. All unspecified parameters are set to reasonable default values.
 
A <code>MotionWaypointOptions</code> object can be passed to <code>MotionWaypoint.set_waypoint_options()</code>, or you can use the <code>to_msg()</code> method to directly use the ROS message format. All unspecified parameters are set to reasonable default values.
 +
 +
For joint trajectories, you can specify the <code>label</code>, <code>max_joint_speed_ratio</code>, <code>joint_tolerances</code>, and <code>max_joint_accel</code> per joint.
 +
 +
For Cartesian trajectories, you can specify the <code>max_linear_speed</code>, <code>max_linear_accel</code>, <code>max_rotational_speed</code>, and <code>max_rotational_accel</code>. The <code>corner_distance</code> parameter can be set to a non-zero value for continuous Cartesian motion; this parameter is the distance from the waypoint to where the curve starts while blending from one straight line segment to the next.
 +
  
 
== Interaction Options==
 
== Interaction Options==
Line 199: Line 250:
  
 
</div>
 
</div>
 
 
  
 
<div class="content-block">
 
<div class="content-block">
Line 207: Line 256:
 
'''New in Intera 5.2!'''
 
'''New in Intera 5.2!'''
  
Interaction control mode is an advanced control mode. In interaction control mode, either impedance control or force control is performed at the endpoint. Also, having 7 joints in Sawyer arm allows movements in the arm without affecting the endpoint pose, which corresponds to nullspace motion. So, independently of the control at the endpoint, stiffness control in the nullspace can be performed as well. The following figure illustrates the interaction control at the endpoint as well as the nullspace.  
+
Interaction control mode is an advanced control mode where either impedance control or force control is performed at the endpoint. Sawyer's seven joints allows movements in the arm without affecting the endpoint pose, which corresponds to nullspace motion. So, stiffness control in the nullspace can be performed independent of the endpoint control. The following figure illustrates the interaction control at the endpoint as well as the nullspace.  
  
 
[[File:Impedance_Control_Concept_Sketch.png|450px]]
 
[[File:Impedance_Control_Concept_Sketch.png|450px]]
Line 214: Line 263:
 
== Impedance Control Concept ==
 
== Impedance Control Concept ==
  
In general, impedance control models the dynamic behavior of the endpoint of the arm as a mass-spring-damper system. In interaction control for Sawyer, the endpoint of the arm is modeled as a spring-damper system as
+
In general, impedance control models the dynamic behavior of the endpoint of the arm as a mass-spring-damper system. The endpoint's spring-damper equation, with the spring constant K and damping constant D, (in the x-axis) is defined as:
  
 
[[File:ImpedanceControlEquationRevised1.png|400px]]
 
[[File:ImpedanceControlEquationRevised1.png|400px]]
  
Additionally, nullspace stiffness control torques can be computed as
+
Additionally, nullspace stiffness control torques can be computed as:
  
 
[[File:ImpedanceControlEquationRevised2.png|300px]]
 
[[File:ImpedanceControlEquationRevised2.png|300px]]
  
Using the above equation, in each of 6 Cartesian axes, desired interaction force can be computed from desired and actual endpoint velocities and positions (and orientations). The computed force can then be used to compute desired joint torques as
+
Using the above equation, in each of the six Cartesian axes, desired interaction forces can be computed from desired and actual endpoint velocities and pose. The computed force can then be used to compute desired joint torques as
  
 
[[File:ImpedanceControlEquationRevised3.png|230px]]
 
[[File:ImpedanceControlEquationRevised3.png|230px]]
  
It can be noted that the magnitude of this impedance force linearly increases as the actual position goes away from the desired one, which means that if the end-effector is blocked somehow, then the applied force could become very large. This can be dangerous and lead to breakage of the parts or environment. To prevent this, we provide <code>IMPEDANCE_WITH_FORCE_LIMIT_MODE</code> which limits the impedance force to a specified limit, and the applied force during interaction can be ensured to be below the limit.  
+
The magnitude of this impedance force linearly increases as the actual position goes away from the desired position. If the end-effector is blocked somehow, then the applied force could become very large which could lead to very high applied forces and dangerous motions. To prevent this, we provide <code>IMPEDANCE_WITH_FORCE_LIMIT_MODE</code> which limits the impedance force to a specified limit where the applied force during interaction can be ensured to be below the limit.  
  
Note that for rotational stiffness control, a similiar control law is constructed based on the orientation error represented by quaternions. By default, the maximum amount of orientation error is limited by a virtual wall, but this limit can be disabled by setting  <code>rotations_for_constrained_zeroG</code> to <code>True</code>.  
+
For rotational stiffness control, a similar control law is constructed based on the orientation error represented by quaternions. By default, the maximum amount of orientation error is limited by a virtual wall, but this limit can be disabled by setting  <code>rotations_for_constrained_zeroG</code> to <code>True</code>.  
  
  
 
== Force Control Concept ==
 
== Force Control Concept ==
  
In force control, the desired force can be specified in each of Cartesian axes. The units are [N] and [Nm] for the first three translational directions and the last three rotational directions, respectively.  
+
In force control, the desired force can be specified in each of the Cartesian axes. The units are Newtons for the first three translational directions and Newton-meters the last three rotational directions.  
  
Force control is supposed to be used only during contact since applying force in free space would generate acceleration at the endpoint, which can be dangerous. So for safety reasons a damping control force is added by default to limit the velocity during the force control, and it can be disabled by setting <code>disable_damping_in_force_control</code> to <code>True</code>.  
+
Force control should be used only during contact because applying force in free space could generate high acceleration at the endpoint, which can be dangerous. For safety reasons, a damping control force is added by default to limit the velocity during the force control, and it can be disabled by setting <code>disable_damping_in_force_control</code> to <code>True</code>.  
  
Also, the force control does not have notion of limits to range of motion, which means that the end-effector will move as long as the joints do not reach their limits if force control is done only in free space. If it is desired to limit the amount of changes in position and orientation during force control, <code>FORCE_WITH_MOTION_LIMIT_MODE</code> can be used. Note that this mode is experimental at this point, and the SDK interface provides no parameters to specify its motion limits. Currently the limits are set to <code>0.12[m]</code> and <code>0.2618[rad]</code>.
+
Also, the force control does not have a range of motion limit, which means that the end-effector will move as long as the joints do not reach their limits if done in free space. If it is desired to limit the amount of change in position and orientation during force control, <code>FORCE_WITH_MOTION_LIMIT_MODE</code> can be used. Note that this mode is experimental at this point, and the SDK interface provides no parameters to specify its motion limits. Currently the limits are set to <code>0.12 [m]</code> and <code>0.2618 [rad]</code>.
  
 
== Interaction Control Overview ==
 
== Interaction Control Overview ==
  
In interaction control mode, the user can specify interaction parameters along with joint angles from which the endpoint pose is computed. These inputs are then subscribed to by Sawyer's Interaction Controller. The interaction controller processes these inputs and generate interaction torques ensuring safety (collision avoidance and detection) and expected behavior by making the following modifications:
+
In interaction control mode, the user can publish interaction parameters along with joint angles from which the endpoint pose is computed. The Interaction Controller subscribes to the interaction control command. The Interaction Controller generates interaction torques and can make the following modifications to account for gravity and to avoid self collisions:
  
 
[[File:Diagram_Interaction_Controller_Revised.png|600px]]
 
[[File:Diagram_Interaction_Controller_Revised.png|600px]]
Line 327: Line 376:
 
</syntaxhighlight>
 
</syntaxhighlight>
  
This message is subscribed to by the <code>realtime_loop</code>.
+
This message is subscribed to by the <code>realtime_loop</code> or can be included in the trajectory options sent to the Motion Controller.
  
 
<code>interaction_control_active</code>:
 
<code>interaction_control_active</code>:
Line 361: Line 410:
 
<code>disable_reference_resetting</code>:
 
<code>disable_reference_resetting</code>:
 
The <code>disable_reference_resetting</code> is a boolean. To avoid jumps/jerks in the event of a change in interaction parameters, the interaction controller resets the reference position to the actual position. This feature can be disabled by setting the variable <code>disable_reference_resetting</code> to True.  
 
The <code>disable_reference_resetting</code> is a boolean. To avoid jumps/jerks in the event of a change in interaction parameters, the interaction controller resets the reference position to the actual position. This feature can be disabled by setting the variable <code>disable_reference_resetting</code> to True.  
 
  
 
</div>
 
</div>
 
  
 
<div class="content-block">
 
<div class="content-block">
  
 
= Joint Trajectory Action Server =
 
= Joint Trajectory Action Server =
Sawyer's SDK comes equipped with Joint Trajectory Action Server (JTAS) to facilitate users needing to command either (or both) of Sawyer's arm through multiple waypoints. These waypoints are supplied as positions, velocities, and/or accelerations for every joint in Sawyer's arm. These waypoints are accompanied by a desired timeframe within which the trajectory must complete. The main benefit of this server is its ability to interpolate between supplied waypoints, command Sawyer's joints accordingly, and ensure the trajectory is being followed within a specified tolerance. To read more about JTA Servers in general, please see the ROS Wiki page on [http://wiki.ros.org/joint_trajectory_action Joint Trajectory Action]. To jump straight to the source code, see the [https://github.com/RethinkRobotics/intera_sdk/blob/master/intera_interface/src/joint_trajectory_action/joint_trajectory_action.py intera_interface JTAS page] on GitHub.
+
If not using the Motion Controller, Sawyer's SDK comes equipped with Joint Trajectory Action Server (JTAS) to facilitate users commanding Sawyer's arm through multiple waypoints. These waypoints are supplied as joint positions, velocities, and/or accelerations, accompanied by a desired trajectory timing. The main benefit of this server is its ability to interpolate between supplied waypoints, command Sawyer's joints accordingly, and ensure the trajectory is being followed within a specified tolerance. To read more about JTA Servers in general, please see the ROS Wiki page on [http://wiki.ros.org/joint_trajectory_action Joint Trajectory Action]. To jump straight to the source code, see the [https://github.com/RethinkRobotics/intera_sdk/blob/master/intera_interface/src/joint_trajectory_action/joint_trajectory_action.py intera_interface JTAS page] on GitHub.
  
 
== JTAS Modes ==
 
== JTAS Modes ==
The JTAS has three modes: Velocity, Position, and Feed Forward Position Mode, 'position', 'velocity' Position, which are invoked as arguments when calling the action server:<br />
+
The JTAS has three modes which are invoked as arguments when calling the action server:<br />
 
'''$''' <code>rosrun intera_interface joint_trajectory_action_server.py</code>
 
'''$''' <code>rosrun intera_interface joint_trajectory_action_server.py</code>
 
* Velocity Mode (<code>--mode velocity</code>)<br />
 
* Velocity Mode (<code>--mode velocity</code>)<br />
Line 384: Line 431:
 
=== Position Mode ===
 
=== Position Mode ===
  
This mode allows for simple trajectory execution of position-only trajectories. This mode is significantly more accurate than velocity mode, at a cost of a speed reduction.
+
This mode allows for simple trajectory execution of position-only trajectories. This mode is significantly more accurate than velocity mode, at a cost of a speed  
 +
reduction.
 +
 
 +
'''Note:''' in Intera 5.2 and beyond, we strongly recommend using the [[#Motion Controller Interface | motion interface]], instead of Position Mode, for smoother arm motions.
 +
 
 +
=== Inverse Dynamics Feed Forward Position Mode (using Joint Trajectory Position Mode) ===
 +
This mode allows the highest speed movement with the most accuracy. By supplying desired joint positions, velocities, and accelerations, Sawyer can anticipate and correct for the dynamics of its movement by calculating "feeding forward" torques in the Motor Controller to accurately execute trajectories.
 +
 
 +
This mode accepts trajectories with Position, Velocity, and/or Acceleration supplied for each joint. [http://moveit.ros.org/ MoveIt!] supplies these three dimensions by default, so it is highly recommended that you use this mode when using motion planners from MoveIt.
 +
 
 +
</div>
 +
 
 +
<div class="content-block">
 +
 
 +
= Collision Avoidance =
 +
 
 +
There are a two levels of collision avoidance models used for Sawyer: tight shapes and collision avoidance shapes. The collision blocks can be visualized in RVIZ by setting the Planned Path field under MotionPlanning and enabling the field Collision Enabled. (Enabling the Collision Enabled field for the RobotModel will also work.)
 +
 
 +
<gallery widths=480px heights=350px perrow=2 mode="nolines" caption="Tight collision shapes">
 +
File:Collision_tight_shapes_view2.png
 +
File:Collision_tight_shapes_view3.png
 +
</gallery>
 +
 
 +
In default (tight) collision model, shown in the figure above, each of Sawyer's link has its own collision shape slightly larger than the size of that link. The urdf for this model is published in the rosparam <code>/robot_description</code>. This model is used by the Motion Controller to check for self collisions along the trajectory prior to motion. This model can also be used by MoveIt! or other external motion planners.
 +
 
 +
In addition to the arm collision shapes show in the images, collision boxes are added around the base of the pedestal and around the controller box attached to the pedestal. These shapes can be used by external motion planners, but are not used by the Motion Controller or collision avoidance.
 +
 
 +
<gallery widths=480px heights=350px perrow=2 mode="nolines" caption="Collision Avoidance shapes">
 +
File:Collision_avoidance_shapes_view2.png
 +
File:Collision_avoidance_shapes_view3.png
 +
</gallery>
 +
 
 +
The larger collision avoidance shapes are published in the rosparam <code>/robot/armnav_robot_description</code>. When these block comes into contact with each other, collision avoidance model is triggered. The collision avoidance torques are calculated proportional to the overlapping distance between shapes. Certain collision shape pairs, such as adjacent links, are not used for collision avoidance.
  
=== Inverse Dynamics Feed Forward Position Mode (using Join Trajectory Position Mode) ===
+
To disable the collision avoidance, publish an empty message at greater than 5 Hz to the topic, <code>/robot/limb/right/suppress_collision_avoidance</code>. The collision avoidance will stay suppressed as long as this is being published.
This mode can allow for the highest speed movement with the most accuracy. By supplying desired Positions, Velocities, and Accelerations for each joint, you give Sawyer to anticipate and correct for the dynamics of its movement by "feeding forward" Velocities and Accelerations to the Motor Controller. This enables Sawyer to use both feedback from its spring deflection sensors, and feed forward from dynamics of the anticipated trajectory to accurately execute trajectories.
 
  
This mode accepts trajectories with Position, Velocity, and/or Acceleration supplied for each joint. Position is required, but Velocity and Acceleration are optional. Supplying Position alone will give you movement very similar to regular Position mode. If the optional Velocities and Accelerations are supplied, they are splined together with Position, and then commanded over the <code>/robot/inverse_dynamics</code> topic. [http://moveit.ros.org/ MoveIt!] supplies these three dimensions by default, so it is highly recommended that you use this mode when using that motion planner.
+
For eg, the command to suppress collision avoidance for Sawyer would be:
 +
<syntaxhighlight lang="bash" enclose="div">
 +
$ rostopic pub /robot/limb/right/suppress_collision_avoidance std_msgs/Empty -r 10
 +
</syntaxhighlight>
  
 
</div>
 
</div>
  
 
<div class="content-block">
 
<div class="content-block">
= Collision Model =
 
  
There are a total of three levels of collision models in Sawyer. <br />
+
= Collision Detection  =
[[File:Collision model.png | 720x480px]]<br /><br />
+
 
As in the figure above, each of Sawyer's link has its own collision block which is slightly bigger than the size of that link. When these block comes into contact with each other, collision avoidance model is triggered. The collision blocks can be visualized in RVIZ by setting the Planned Path field under MotionPlanning and enabling the field Collision Enabled.<br />
+
There is three types of collisions detected by the robot: impact, squish, and hand over-wrench.  
The next collision model involves the detection of two types of collisions - impact and squish. Impact is detected when a sudden change in torque is sensed across any of the joint. This can be related to a scenario in which a moving hand collides with a human. Here, the sudden change in torque is sensed during the impact and the robot comes to a stop for 2 seconds before attempting to move again. Squish is detected when joint tries to press against a stationary object. For instance, when a robot arm tries to push a wall, the torque applied across the joint increases and it immediately stops when the applied torque is greater than a threshold. It resumes its motion after 2 seconds. <br />
+
 
 +
<code>Impact</code> is detected when a sudden change in torque is sensed across any of the joint. This can be related to a scenario in which a moving robot collides with a human. Here, the sudden change in torque is sensed during the impact and the robot comes to a stop for half a second before attempting to move again.  
 +
 
 +
<code>Squish</code> is detected when joint tries to press against a stationary object. For instance, when a robot arm tries to push a wall, the torque applied across the joint increases and it immediately stops when the difference between measured and expected joint torque is greater than a threshold. Squish detection remains active as long as something is pushing on the arm.
 +
 
 +
<code>Hand Over-wrench</code> is detected when the forces and torques at the robot's hand is excessive. The arm is likely to detect a squish, if active, before an hand over-wrench detection. The arm will stop after a hand over-wrench detection and will not move for half a second.
 +
 
 +
'''Warning:''' For trajectories being sent directly by SDK users, you are responsible to stop sending motion commands. Otherwise, there could be large jumps in the arm motion after the collision detection is finished. The Motion Controller and Joint Action Services already do this.
 +
 
 
The rethink log records these information.
 
The rethink log records these information.
  
Line 413: Line 501:
 
</syntaxhighlight>
 
</syntaxhighlight>
  
These collision information can also be viewed in rqt_console. The images below show the impact and squish logged in rqt_console. <br /><br />
+
These collision information can also be viewed in rqt_console. The images below show the impact and squish logged in rqt_console.
[[File:Impact.png| 720x480px]]<br /><br />
+
 
[[File:Squish.png| 720x480px]]
+
[[File:Impact.png| 720px]]
 +
 
 +
[[File:Squish.png| 720px]]
  
The final collision model is high speed scaling. This is to avoid the collisions of right limb moving at higher speeds. The collision blocks for this purpose is higher than the orginal collision blocks. The collision detection happens when the speed of the limb in cartesian space is higher than 0.2 m/s. In order to avoid the collision, the corresponding velocities are scaled down and converted back as position commands. <br />
 
  
To disable the collision avoidance on a right arm, you need to publish an empty message at greater than 5hz to the topic, <code>/robot/limb/right/suppress_collision_avoidance</code>
+
To disable the collision detection (squish and impact), publish an empty message at greater than 5 Hz to the topic <code>/robot/limb/right/suppress_contact_safety</code>. The squish and impact detection will stay suppressed as long as this is being published.
  
For eg, the command to suppress collision avoidance on the right arm would be:
+
For eg, the command to suppress collision detection for Sawyer would be:
 
<syntaxhighlight lang="bash" enclose="div">
 
<syntaxhighlight lang="bash" enclose="div">
$ rostopic pub /robot/limb/right/suppress_collision_avoidance std_msgs/Empty -r 10
+
$ rostopic pub /robot/limb/right/suppress_contact_safety std_msgs/Empty -r 10
 
</syntaxhighlight>
 
</syntaxhighlight>
The collision avoidance will stay suppressed as long as this is being published
+
 
 
</div>
 
</div>
  
Line 432: Line 521:
 
= Gravity Compensation Torques =
 
= Gravity Compensation Torques =
  
In order to oppose the effect of gravitational force acting across Sawyer's arm, gravity compensation torques have to be applied across that arm. This is a basic mode that is active from the time the robot is enabled. The built-in gravity compensating model uses [http://www.orocos.org/kdl KDL] for calculating the gravity compensation torques, these are found using an internal spring model and applied in conjunction with the torques from KDL. These gravity compensation torques are passed directly to the JCB through a separate channel and are applied irrespective of the controller being active.<br />
+
In order to oppose the effect of gravitational force acting across Sawyer's arm, gravity compensation torques have to be applied across that arm. The built-in gravity compensating model uses [http://www.orocos.org/kdl KDL] for calculating the gravity compensation torques. In addition, inertial feed-forward and interaction torques are calculated depending on which controller is active. These torques are applied before sending joint commands to the Joint Control Boards.
  
 
The gravity compensation torques are available via the topic <code>robot/limb/right/gravity_compensation_torques</code> of message type <code>intera_core_msgs/SEAJointState</code>
 
The gravity compensation torques are available via the topic <code>robot/limb/right/gravity_compensation_torques</code> of message type <code>intera_core_msgs/SEAJointState</code>
  
 
<syntaxhighlight lang="bash" enclose="div">
 
<syntaxhighlight lang="bash" enclose="div">
$ rostopic echo robot/limb/right/gravity_compensation_torques # of message type intera_core_msgs/SEAJointState
+
$ rostopic echo robot/limb/right/gravity_compensation_torques
 
</syntaxhighlight>
 
</syntaxhighlight>
  
Line 462: Line 551:
 
actual_effort: Measured Effort from RealTime Loop via the Spring Deflection Sensors in each Series Elastic Actuator
 
actual_effort: Measured Effort from RealTime Loop via the Spring Deflection Sensors in each Series Elastic Actuator
  
gravity_model_effort: Gravity Compensation Torques Commanded
+
gravity_model_effort: Gravity Compensation Torques Commanded (includes the inertial feed forward torques when applicable)
 +
 
 +
gravity_only: Torque required to hold the arm against gravity returned by KDL if the arm was stationary
 +
 
 +
interaction_torque: Torque produced by the Interaction Controller plugin
  
hysteresis_model_effort: Spring Compensation Torques Commanded. This is the hysterisis effort applied to account for spring joint j1
+
hysteresis_model_effort: Spring Compensation Torques Commanded. [Not applicable to Sawyer]
  
crosstalk_model_effort: Joint Crosstalk Compensation. Present here is minimal compensation for joint j1 crosstalk caused by the arm's downstream joints
+
crosstalk_model_effort: Joint Crosstalk Compensation. [Not applicable to Sawyer]
  
hystState: A variable used in calculating the hysteresis model.
+
hystState: A variable used in calculating the hysteresis model. [Not applicable to Sawyer]
 
</syntaxhighlight>
 
</syntaxhighlight>
 
</div>
 
</div>
Line 476: Line 569:
 
= Zero-G mode =
 
= Zero-G mode =
  
Zero-G mode can often be confused with the mode obtained by disabling the gravity compensation torques. By default, the gravity compensation torques will always be applied when the robot is enabled. In Zero-G mode, the controllers are disabled and so the arm can be freely moved across. In this case, the effect of gravity would be compensated by the gravity compensation model applying gravity compensation torques across the joints, there would be no torques from the controllers since they would not be active, and so the arm can be moved freely around, hence the name.<br />
+
Zero-G mode allows the user to free move the arm. In this mode, the joint position controllers are disabled while the gravity compensation torques are still active to reduce the gravitational forces on the arm.  
  
The Zero-G mode can be enabled by grasping the cuff over its groove or press circle button on navigators,
+
The Zero-G mode can be enabled by grasping the cuff over its groove or pressing the circle button on the navigators,
  
 
[[File:Sawyer_Training_Cuff.png|500px]] [[File:SDK Navigator.png|500px]]
 
[[File:Sawyer_Training_Cuff.png|500px]] [[File:SDK Navigator.png|500px]]
  
 +
'''Note:''' The estimated gravitational torques depends on the accuracy on the arm's kinematic model, link mass properties, and torque sensing. Because the estimated torques are not perfect, the arm may drift if the cuff is not firmly held while zero-g mode is active. If there is a strong pull, try running the [[Arm_Calibration|arm  calibration routine]].
  
 
Unlike the gravity compensation model, the Zero-G mode cannot be directly disabled. The cuff interaction has to be disabled in order to disable the Zero-G. Publishing an empty message to the topic <code>/robot/limb/right/suppress_cuff_interaction</code> at greater than 5Hz, disabled the cuff interaction.
 
Unlike the gravity compensation model, the Zero-G mode cannot be directly disabled. The cuff interaction has to be disabled in order to disable the Zero-G. Publishing an empty message to the topic <code>/robot/limb/right/suppress_cuff_interaction</code> at greater than 5Hz, disabled the cuff interaction.

Latest revision as of 20:28, 29 March 2019

Control Overview

There are two primary workflows for moving the arm via the SDK. The first workflow sends high frequency joint commands to the realtime motor controller via one of four control modes. The second workflow is the motion interface, introduced in the Intera 5.2 software release, which sends trajectory planning request to the Motion Controller to generate and execute a smooth trajectory between waypoints.

An additional new feature in Intera 5.2 is the interaction control mode, which can implement either an impedance controller or force controller at the endpoint of the arm. Interaction control mode can be used by itself, or used in conjunction with the motion interface to generate compliant behavior along a trajectory.


Joint Command Overview

SDK Interface Drawing Workflow 1.png

Moving the arm through joint commands is appropriate for users who either plan their own motion trajectories or want fine level control of the arm. In this workflow, the user is responsible for planning the arm's trajectory and timing, often via a MoveIt! planner or a customer arm controller.

When using joint commands, the arm control flows through on four layers of operation:

1) User Code running via workstation (Python Joint Control or Joint Trajectory Action Server or your custom interface)
2) Joint Control Listeners via ROS topic (on Sawyer's internal Ubuntu Linux PC)
3) RealTime Motor Control Loop (the highest priority process on Sawyer's internal Ubuntu Linux PC)
4) Joint Control Boards (microcontrollers attached to each arm joint)

The bottom three layers (2-4) are not accessible for user modification because they run on the robot itself and its microcontrollers. Sawyer does have four joint control modes for the user to choose from at the top level: position, trajectory, velocity, and torque control. Each controller has a subset of Motor Controller Plugins associated with them. By default, all the joint control publishers and subscribers provided by the Sawyer SDK run at 100Hz, but can be run up to a rate of 800Hz reliably. See the links for more detailed explanations.


Motion Controller Interface Overview

New in Intera 5.2!

SDK Interface Drawing Workflow 2.png

Users who want basic motions without generating their own trajectories should use the Motion Controller interface. The motion controller interface is a ROS action server that allows the SDK access to built-in trajectory generation and motion control features between widely spaced waypoints. Smooth trajectories can be created in either joint mode (move quickly between waypoints) or cartesian mode (end-effector moves along straight lines). Interaction control parameters can be included with the options for each waypoint to allow for compliant behavior along a trajectory. Note that the Motion Controller will not allow trajectories that result in self collision. However, it does not know about end-effector collision shapes or obstacles in the environment, so these are not considered when running the collision check.


RealTime Motor Controller

The RealTime Motor Control loop runs on the robot and executes over the course of 1ms, and approximately 700us of this loop is used on average. Occasionally you may find an overrun of the RealTime loop around 100us. This overrun usually does not propagate forward to other cycles because the next loop will likely take 700us. To verify this you can look at the diagnostics messages via "rostopic echo /diagnostics", and look for the "Realtime Control Loop".

The RealTime Motor Controller has plugins that run simultaneously with joint commands. These are Collision Avoidance, Collision Detection and Gravity Compensation. On top of these plugins, when Sawyer's cuff pressure sensor is activated, Zero-G Mode enables, allowing the arm to be passively back driven. Upon releasing the cuff, the motor controller returns to the controller it was using previously. For more detailed log information, either view the log messages in the rqt_console, by filtering for the realtime_loop node, or see the realtime_loop-stdout.log export for more details.

Joint Control Modes

Fundamental to the operation of Sawyer via the SDK is a familiarity with the available joint control modes. The joint control modes define the 'modes' in which we can control Sawyer's limb in joint space. These are provided via a ROS message API:(intera_core_msgs/JointCommand.msg)

To control the limb, we must publish the JointCommand message on the topic:

robot/limb/right/joint_command

at a rate > 5hz (by default). The rate at which you must publish your messages is defined on the topic:

/robot/limb/right/joint_command_timeout

of message type std_msgs/Float64


Message Definition

The JointCommand message is defined:

Header header

int32 mode             # Mode in which to command arm

string[]  names        # Joint names order for command

# Fields of commands indexed according to the Joint names vector.
# Command fields required for a desired mode are listed in the comments
float64[] position     # (radians)       Required for POSITION_MODE and TRAJECTORY_MODE
float64[] velocity     # (radians/sec)   Required for VELOCITY_MODE and TRAJECTORY_MODE
float64[] acceleration # (radians/sec^2) Required for                   TRAJECTORY_MODE
float64[] effort       # (newton-meters) Required for TORQUE_MODE

# Modes available to command arm
int32 POSITION_MODE=1
int32 VELOCITY_MODE=2
int32 TORQUE_MODE=3
int32 TRAJECTORY_MODE=4

This message is subscribed to by the realtime_loop.

mode: The mode is an integer defining the mode in which the commands will be parsed and commanded to the Joint Controller Boards (JCBs). Available modes are contained with the message [POSITION_MODE, VELOCITY_MODE, TORQUE_MODE, TRAJECTORY_MODE]

command field: The command is the ordered (corresponding to the names field) command values.

names: The names is a list of strings containing the joints to command.


Joint Control Fundamentals

The 'realtime_loop' process subscribes to joint position commands published from the development workstation. Modifications, based on control mode, can be made to the input commands. These modifications are typically due to the safety controllers (e.g. arm-to-arm collision avoidance, collision detection, etc.)

A control rate timeout is also enforced at this motor controller layer. If a new 'JointCommand' message is not received within the specified timeout (default of 0.2 seconds, or 5Hz), the robot will 'Timeout'. When the robot 'Times out', the current control mode is exited, reverting back to position control mode where the robot will command (hold) it's current joint angles. This timeout can prevent dangerous motions due to network issues (e.g. someone kicks your router).

This control rate timeout is configurable:

/robot/limb/right/joint_command_timeout

of message type std_msgs/Float64

Joint Position Control

Depreciated for Intera 5.2 and beyond: use the Motion Controller Interface instead.

Joint position control mode is a simple control mode for Sawyer arm motion. In position control mode, we specify joint angles at which we want the joints to achieve. Typically this will be consist of seven values, a commanded position for each of the seven joints, resulting in a full description of the arm configuration. This control mode can result in jerky-motions and should generally be avoided.

JointCommand.msg mode: POSITION_MODE

This joint command is then subscribed to by Sawyer's Motor Controller. The motor controller processes this joint command ensuring safety (collision avoidance and detection) and expected behavior by making the following modifications:

Joint Position Control.png

Joint Trajectory-Position Control

Joint Trajectory-Position Control mode provides the most direct way for the robot to follow a time-based reference trajectory. The user must supply position, velocity, and acceleration in each command, which results in a far smoother motion than the Joint Position Control mode. The Realtime loop will attempt to move to the desired joint angles position, with the desired joint velocity. The joint accelerations (along with commanded position and velocity) are used to extrapolate between successive commands and to generate feed-forward torque terms to improve the dynamic trajectory tracking performance.

JointCommand.msg mode: TRAJECTORY_MODE

The Joint Trajectory-Position commands only perform the following modification (typically these additions are none):

'Raw' Joint Position Control.png

Warning: Advanced Control Mode. As this mode does not have Collision Avoidance offsets, and allows for very fast motions at the joints velocity limits, this mode can be dangerous. Please use with caution.

Joint Velocity Control

Joint velocity control mode is an advanced control mode. In velocity control mode, we specify joint velocities at which we want the joints to simultaneously achieve. Typically this will be consist of seven commanded joint velocities.

JointCommand.msg mode: VELOCITY_MODE

This joint command is then subscribed to by Sawyer's Motor Controller. The motor controller processes this joint velocity command (applying collision avoidance and detection) and expected behavior by making the following modifications:

Joint Velocity Control.png

Note: We recommend either implementing a joint space potential field or joint limit (position and velocity) check before submitting the joint velocity commands.

Warning: Advanced Control Mode. As we allow for very fast joint velocity up to the joint velocity limits, this mode can be dangerous. Please use with caution.


Joint Torque Control

Joint torque control mode is an advanced control mode. In torque control mode, we specify joint torques which we want the joints to simultaneously achieve. Typically this will be consist of seven values, a commanded torque for each of the seven joints.

JointCommand.msg mode: TORQUE_MODE

This joint command is then subscribed to by Sawyer's Motor Controller. The motor controller processes this joint torque command and expected behavior by making the following modifications, note that collision avoidance torques are still active to avoid self collisions:

Joint Torque Control.png

Note: As shown in the above image, joint torque commands are applied in addition to gravity torques. This default can be disabled by publishing an std_msgs/Empty message on the topic: /robot/limb/right/suppress_gravity_compensation at a rate > 5Hz.

Warning: Advanced Control Mode. When commanding in torque control mode, access is granted to the lowest control levels. This puts much responsibility on the control program and can be dangerous. Please use with caution.


Joint Command Timing

Start to finish, a joint is commanded from the user's python code. This message flows over the ROS network and is picked up by the Joint Control Listeners which store the command for it to be retrieved asynchronously by the RealTime Motor Control Loop once per 1ms (1KHz). The command is then sent to the Joint Control Boards (JCB) and executed within the same 1ms timeslot. On the next 1ms cycle, the joint position and spring deflection sensor are read and transferred back to the Joint Control Publisher. The result is read over the ROS network by the user's code. The total timing looks approximately as follows:

1.6ms for the Command to publish over ROS to the Joint Controller over rostopic (via network)
1ms for the Joint Controller Listener to command the Motor Controller (MC), running an async real time loop of 1KHz
1ms for the MC to transfer to the Joint Control Board (JCB) and evaluate the command
1ms for the JCB to read the updated status and give a response to the MC
1ms for the MC to report back to the Joint Controller Publisher
1.6ms for the Joint Controller to publishing the State back over rostopic (via network)
________
~ 7.2ms total round trip ROS User Publisher to ROS user Subscriber

Motion Controller Interface

The motion controller interface allows the SDK access to built-in trajectory generation and motion control features in Sawyer. The python intera motion interface provides a set of classes used to construct intera motion messages as well as an ROS action client which communicates with the Motion Controller on the robot controller. The Motion Controller will then generate and run a motion plan from that trajectory message.


ROS Action Server

The Motion Controller has a ROS Action server on the topic:

/motion/motion_command

The goal message has a string command which can be either "start" or "stop" along with a intera_motion_msgs/Trajectory message to define the desired motion. The intera_motion_msgs/MotionStatus provides feedback during the trajectory which includes the motion_status along with the current_trajectory and current_waypoint ids. A result message is returned after either a successful or failed trajectory completion; it includes a boolean result and string errorID if needed. Trajectories will stop if a collision is detected by the robot or the robot is disabled.

The Motion Controller will generate a trajectory through all of the waypoints starting at the current joint configuration and stopping at the last waypoint. The planner will avoid stopping at the middle waypoints when possible. If a new trajectory is sent before the current one is finished, then the current motion will stop. The new trajectory will be generated starting from the current joint positions (where the arm stopped moving). The trajectory interpolation type can be set to either Joint or Cartesian; mixed trajectory interpolation types are not allowed.

Before executing a planned motion, the Motion Controller first sub-samples the path to check for self collisions (not including external obstacles or end effector collision shapes). If a planned collision is found, the Motion Controller will return a PLANNED_MOTION_COLLISION error in the result message. To disable collision checking, publish an empty message at greater than 5 Hz to the topic /robot/limb/right/suppress_collision_avoidance.

New in Intera 5.3: When the motion controller receives a stop command while running a motion it will now perform a smooth path-following stop over 300 milliseconds.

Trajectory Interpolation: Joint vs. Cartesian

The motion controller supports two basic methods for interpolation between waypoints: JOINT mode and CARTESIAN mode. Joint mode should generally be preferred to Cartesian mode, as it results in motion that is faster and smoother than Cartesian mode.

JOINT MODE: For trajectories with joint interpolation, the motion controller will generate a fast and smooth path between waypoints. It may or may not stop at intermediate waypoints, and the path is generally non-linear in joint space (unless there is a single waypoint).

CARTESIAN MODE: The motion planning for Cartesian trajectories occurs in two steps: 1) A trajectory is generated in cartesian space that moves linearly (position + orientation) between each waypoint. If the blending radius option is enabled, then the planner will allow these linear paths to be connected with circular arcs and the motion will not stop as it passes near each waypoint. 2) An inverse-kinematics solver is used to compute the joint angles to move the endpoint along the reference trajectory. If joint angle are provided along with the Cartesian poses in the waypoints, the joint angles will be used to bias the IK solutions along each trajectory segment. Note that the active_endpoint must be consistent for all waypoints within a trajectory.

Possible Errors

Here is a list of possible errors which could be returned from the Motion Controller action server in the result message. For more detailed information from a reported error, either view the log messages in the rqt_console, by filtering for the motion_controller node, or see the motion_controller-stdout.log export for more details.

INVALID_TRAJECTORY_MESSAGE 
The trajectory message was either missing necessary parameters or the parameters were inconsistent
FAILED_TO_PARAMETERIZE 
A valid joint trajectory spline could not be generated
PLANNED_MOTION_COLLISION 
Prior to moving, a self-collision was found along the planned trajectory
ENDPOINT_DOES_NOT_EXIST 
For a Cartesian trajectory, the endpoint string was not found in the current URDF
CARTESIAN_INTERPOLATION_FAILED 
A valid Cartesian trajectory could not be generate or there was an Inverse Kinemaics solution could not be found
PLANNED_JOINT_ACCEL_LIMIT 
The generated Cartesian trajectory would violate joint limits. Try reducing the Cartesian speeds and accelerations.
FINAL_POSE_NOT_WITHIN_TOLERANCE 
The trajectory finished, but the joints did not settle within the desired joint tolerance
CONTROLLER_NOT_FOLLOWING 
The trajectory was stopped because either the arm detected a collision or the arm was disabled
ZERO_G_ACTIVATED_DURING_TRAJECTORY 
The trajectory was stopped because zero-g was activated mid-motion

Trajectory Message

A intera_motion_msgs/Trajectory message is used to specify a trajectory for Sawyer. It contains a sequence of waypoints that the trajectory should pass through, as well as the interpolation type, kinematic constraints, and interaction parameters. The Motion Controller always appends the current arm configuration to the beginning of the trajectory. Generally the trajectory message is created using the MotionTrajectory class, but advanced users might choose to create it directly. A trajectory message can be sent to the motion controller using either the MotionTrajectory class (preferred method) or the MotionControllerActionClient class (advanced users).


Motion Trajectory

The MotionTrajectory class is used as a utility to create and send intera_motion_msgs/Trajectory messages. The simplest way to use it is to call the append_waypoint() to build up a sequence of waypoints and then call send_trajectory() to plan and execute the a trajectory that passes through the waypoints. The MotionWaypoint, MotionWaypointOptions, and InteractionOptions classes are all utilities that help with the construction of trajectories.


Motion Waypoint

The MotionWaypoint class is used as utility to create intera_motion_msgs/Waypoint messages. It can be directly passed to the append_waypoint() method of a MotionTrajectory, or you can use the to_msg() method to directly use the ROS message format. All unspecified parameters are set to reasonable default values.


Motion Waypoint Options

The MotionWaypointOptions class is used as utility to create intera_motion_msgs/WaypointOptions messages. A MotionWaypointOptions object can be passed to MotionWaypoint.set_waypoint_options(), or you can use the to_msg() method to directly use the ROS message format. All unspecified parameters are set to reasonable default values.

For joint trajectories, you can specify the label, max_joint_speed_ratio, joint_tolerances, and max_joint_accel per joint.

For Cartesian trajectories, you can specify the max_linear_speed, max_linear_accel, max_rotational_speed, and max_rotational_accel. The corner_distance parameter can be set to a non-zero value for continuous Cartesian motion; this parameter is the distance from the waypoint to where the curve starts while blending from one straight line segment to the next.


Interaction Options

The InteractionOptions class is used as a utility to create intera_core_msgs/InteractionControlCommand messages. Use the to_msg() method to extract the ROS message for use in populating an intera_motion_msgs/TrajectoryOptions message. All unspecified parameters are set to reasonable default values.

Interaction Control Mode

New in Intera 5.2!

Interaction control mode is an advanced control mode where either impedance control or force control is performed at the endpoint. Sawyer's seven joints allows movements in the arm without affecting the endpoint pose, which corresponds to nullspace motion. So, stiffness control in the nullspace can be performed independent of the endpoint control. The following figure illustrates the interaction control at the endpoint as well as the nullspace.

Impedance Control Concept Sketch.png


Impedance Control Concept

In general, impedance control models the dynamic behavior of the endpoint of the arm as a mass-spring-damper system. The endpoint's spring-damper equation, with the spring constant K and damping constant D, (in the x-axis) is defined as:

ImpedanceControlEquationRevised1.png

Additionally, nullspace stiffness control torques can be computed as:

ImpedanceControlEquationRevised2.png

Using the above equation, in each of the six Cartesian axes, desired interaction forces can be computed from desired and actual endpoint velocities and pose. The computed force can then be used to compute desired joint torques as

ImpedanceControlEquationRevised3.png

The magnitude of this impedance force linearly increases as the actual position goes away from the desired position. If the end-effector is blocked somehow, then the applied force could become very large which could lead to very high applied forces and dangerous motions. To prevent this, we provide IMPEDANCE_WITH_FORCE_LIMIT_MODE which limits the impedance force to a specified limit where the applied force during interaction can be ensured to be below the limit.

For rotational stiffness control, a similar control law is constructed based on the orientation error represented by quaternions. By default, the maximum amount of orientation error is limited by a virtual wall, but this limit can be disabled by setting rotations_for_constrained_zeroG to True.


Force Control Concept

In force control, the desired force can be specified in each of the Cartesian axes. The units are Newtons for the first three translational directions and Newton-meters the last three rotational directions.

Force control should be used only during contact because applying force in free space could generate high acceleration at the endpoint, which can be dangerous. For safety reasons, a damping control force is added by default to limit the velocity during the force control, and it can be disabled by setting disable_damping_in_force_control to True.

Also, the force control does not have a range of motion limit, which means that the end-effector will move as long as the joints do not reach their limits if done in free space. If it is desired to limit the amount of change in position and orientation during force control, FORCE_WITH_MOTION_LIMIT_MODE can be used. Note that this mode is experimental at this point, and the SDK interface provides no parameters to specify its motion limits. Currently the limits are set to 0.12 [m] and 0.2618 [rad].

Interaction Control Overview

In interaction control mode, the user can publish interaction parameters along with joint angles from which the endpoint pose is computed. The Interaction Controller subscribes to the interaction control command. The Interaction Controller generates interaction torques and can make the following modifications to account for gravity and to avoid self collisions:

Diagram Interaction Controller Revised.png


Interaction Control Command

In order to use interaction control mode, we must publish the InteractionControlCommand message on the topic:

/robot/limb/right/interaction_control_command


Message Definition

The Interaction Control Command message is defined:

# Message sets the interaction (impedance/force) control on or off
# It also contains desired cartesian stiffness K, damping D, and force values

Header header
bool      interaction_control_active

## Mode Selection Parameters
# The possible interaction control modes are:
# Impedance mode: implements desired endpoint stiffness and damping.
uint8 IMPEDANCE_MODE=1
# Force mode: applies force/torque in the specified dimensions.
uint8 FORCE_MODE=2
# Impedance with force limit: impedance control while ensuring the commanded
# forces/torques do not exceed force_command.
uint8 IMPEDANCE_WITH_FORCE_LIMIT_MODE=3
# Force with motion bounds: force control while ensuring the current
# pose/velocities do not exceed forceMotionThreshold (currenetly defined in yaml)
uint8 FORCE_WITH_MOTION_LIMIT_MODE=4

# Specifies the interaction control mode for each Cartesian dimension (6)
uint8[] interaction_control_mode

# All 6 values in force and impedance parameter vectors have to be filled,
# If a control mode is not used in a Cartesian dimension,
# the corresponding parameters will be ignored.

## Cartesian Impedance Control Parameters
# Stiffness units are (N/m) for first 3 and (Nm/rad) for second 3 values
float64[] K_impedance
# Force certain directions to have maximum possible impedance for a given pose
bool[] max_impedance
# Damping units are (Ns/m) for first 3 and (Nms/rad) for the second 3 values
float64[] D_impedance
# Joint Nullspace stiffness units are in (Nm/rad) (length == number of joints)
float64[] K_nullspace

## Parameters for force control or impedance control with force limit
# If in force mode, this is the vector of desired forces/torques
# to be regulated in (N) and (Nm)
# If in impedance with force limit mode, this vector specifies the
# magnitude of forces/torques (N and Nm) that the command will not exceed.
float64[] force_command

## Desired frame
geometry_msgs/Pose interaction_frame
string endpoint_name
# True if impedance and force commands are defined in endpoint frame
bool in_endpoint_frame

# Set to true to disable damping during force control. Damping is used
# to slow down robot motion during force control in free space.
# Option included for SDK users to disable damping in force control
bool disable_damping_in_force_control

# Set to true to disable reference resetting. Reference resetting is
# used when interaction parameters change, in order to avoid jumps/jerks.
# Option included for SDK users to disable reference resetting if the
# intention is to change interaction parameters.
bool disable_reference_resetting

## Parameters for Constrained Zero-G Behaviors
# Allow for arbitrary rotational displacements from the current orientation
# for constrained zero-G. Setting 'rotations_for_constrained_zeroG = True'
# will disable the rotational stiffness field which limits rotational
# displacements to +/- 82.5 degree.
# NOTE: it will be only enabled for a stationary reference orientation
bool rotations_for_constrained_zeroG

This message is subscribed to by the realtime_loop or can be included in the trajectory options sent to the Motion Controller.

interaction_control_active: The interaction_control_active is a boolean to activate (True) or deactivate (False) interaction control mode. When it is set to False, the arm will be in position mode.

interaction_control_mode: The interaction_control_mode is an integer defining the internal mode in interaction control mode. Available modes are contained with the message [IMPEDANCE_MODE, FORCE_MODE, IMPEDANCE_WITH_FORCE_LIMIT_MODE, FORCE_WITH_MOTION_LIMIT_MODE]

K_impedance: The K_impedance is a list of desired stiffness values for 6 Cartesian axes (translational directions along x, y, and z axes followed by rotational directions about x, y, and z axes of the reference frame). Each of them must be equal or greater than zero, and they will be capped to their maximum values (1300[N/m] and 30[Nm/rad], respectively) if exceeded.

max_impedance: The max_impedance is a list of booleans for 6 Cartesian axes. For directions whose values are set to True, a maximum stiffness value will be computed and used for the current joint configuration, regardless of the values of K_impedance. The maximum stiffness values are somewhere between 1300[N/m] and 3000[N/m] and 30[Nm/rad]] and 300[Nm/rad] for translational and rotational directions, respectively.

D_impedance: The D_impedance is a list of desired damping values for 6 Cartesian axes. If unspecified, default values are used, which are scaled according to stiffness values. The default damping values are 8[Ns/m] and 2[Nms/rad] for tranlational and rotational directions, respectively.

K_nullspace: The K_nullspace is a list of desired nullspace stiffness values for all 7 joints. Each of them must be equal or greater than zero, and they will be capped to their maximum values (10[Nm/rad]) if exceeded.

force_command: The force_command is a list of desired force values for 6 Cartesian axes. Each of them will be capped to their maximum values (50[N] and 5[Nm] for translational and rotational directions, respectively) if exceeded.

interaction_frame: The interaction_frame is a geometry_msgs/Pose that specifies the position and orientation of the reference frame with respect to which the interaction control behaviors are defined.

endpoint_name: The endpoint_name is a string that specifies the name of the endpoint frame at which the interaction control behaviors are defined. Note that currently this field is not allowed to be modified, and it is always set to right_hand by default.

in_endpoint_frame: The in_endpoint_frame is a boolean, which can be used to set the endpoint frame (i.e., TCP frame) as the reference frame of interaction control behaviors if set to True; Base frame will be used if set to False.

disable_reference_resetting: The disable_reference_resetting is a boolean. To avoid jumps/jerks in the event of a change in interaction parameters, the interaction controller resets the reference position to the actual position. This feature can be disabled by setting the variable disable_reference_resetting to True.

Joint Trajectory Action Server

If not using the Motion Controller, Sawyer's SDK comes equipped with Joint Trajectory Action Server (JTAS) to facilitate users commanding Sawyer's arm through multiple waypoints. These waypoints are supplied as joint positions, velocities, and/or accelerations, accompanied by a desired trajectory timing. The main benefit of this server is its ability to interpolate between supplied waypoints, command Sawyer's joints accordingly, and ensure the trajectory is being followed within a specified tolerance. To read more about JTA Servers in general, please see the ROS Wiki page on Joint Trajectory Action. To jump straight to the source code, see the intera_interface JTAS page on GitHub.

JTAS Modes

The JTAS has three modes which are invoked as arguments when calling the action server:
$ rosrun intera_interface joint_trajectory_action_server.py

  • Velocity Mode (--mode velocity)
  • Position Mode (--mode position)
  • Inverse Dynamics Feed Forward Position Mode (default mode)

Velocity Mode

This mode executes direct control of trajectories by calculating and commanding the velocities required to move the arm through a set of joint positions at a specified time. The end result is fast, but less accurate movement.

Position Mode

This mode allows for simple trajectory execution of position-only trajectories. This mode is significantly more accurate than velocity mode, at a cost of a speed reduction.

Note: in Intera 5.2 and beyond, we strongly recommend using the motion interface, instead of Position Mode, for smoother arm motions.

Inverse Dynamics Feed Forward Position Mode (using Joint Trajectory Position Mode)

This mode allows the highest speed movement with the most accuracy. By supplying desired joint positions, velocities, and accelerations, Sawyer can anticipate and correct for the dynamics of its movement by calculating "feeding forward" torques in the Motor Controller to accurately execute trajectories.

This mode accepts trajectories with Position, Velocity, and/or Acceleration supplied for each joint. MoveIt! supplies these three dimensions by default, so it is highly recommended that you use this mode when using motion planners from MoveIt.

Collision Avoidance

There are a two levels of collision avoidance models used for Sawyer: tight shapes and collision avoidance shapes. The collision blocks can be visualized in RVIZ by setting the Planned Path field under MotionPlanning and enabling the field Collision Enabled. (Enabling the Collision Enabled field for the RobotModel will also work.)

In default (tight) collision model, shown in the figure above, each of Sawyer's link has its own collision shape slightly larger than the size of that link. The urdf for this model is published in the rosparam /robot_description. This model is used by the Motion Controller to check for self collisions along the trajectory prior to motion. This model can also be used by MoveIt! or other external motion planners.

In addition to the arm collision shapes show in the images, collision boxes are added around the base of the pedestal and around the controller box attached to the pedestal. These shapes can be used by external motion planners, but are not used by the Motion Controller or collision avoidance.

The larger collision avoidance shapes are published in the rosparam /robot/armnav_robot_description. When these block comes into contact with each other, collision avoidance model is triggered. The collision avoidance torques are calculated proportional to the overlapping distance between shapes. Certain collision shape pairs, such as adjacent links, are not used for collision avoidance.

To disable the collision avoidance, publish an empty message at greater than 5 Hz to the topic, /robot/limb/right/suppress_collision_avoidance. The collision avoidance will stay suppressed as long as this is being published.

For eg, the command to suppress collision avoidance for Sawyer would be:

$ rostopic pub /robot/limb/right/suppress_collision_avoidance std_msgs/Empty -r 10

Collision Detection

There is three types of collisions detected by the robot: impact, squish, and hand over-wrench.

Impact is detected when a sudden change in torque is sensed across any of the joint. This can be related to a scenario in which a moving robot collides with a human. Here, the sudden change in torque is sensed during the impact and the robot comes to a stop for half a second before attempting to move again.

Squish is detected when joint tries to press against a stationary object. For instance, when a robot arm tries to push a wall, the torque applied across the joint increases and it immediately stops when the difference between measured and expected joint torque is greater than a threshold. Squish detection remains active as long as something is pushing on the arm.

Hand Over-wrench is detected when the forces and torques at the robot's hand is excessive. The arm is likely to detect a squish, if active, before an hand over-wrench detection. The arm will stop after a hand over-wrench detection and will not move for half a second.

Warning: For trajectories being sent directly by SDK users, you are responsible to stop sending motion commands. Otherwise, there could be large jumps in the arm motion after the collision detection is finished. The Motion Controller and Joint Action Services already do this.

The rethink log records these information.

[2017-01-12T14:31:32.807793-0500] [WARN] (/collision/right/joint_collision_estimator_right,JointCollisionEstimator.cpp:746) | impact: 0.885333 > 0.750000, largest contributor: right_j2, with value: 0.480856

This log information indicates that an impact was detected when the torque applied across the right_j2 joint is greater than 0.75 Nm. Similarly the corresponding message logged for squish is

[2017-01-12T14:28:20.933828-0500] [WARN] (/collision/right/joint_collision_estimator_right,JointCollisionEstimator.cpp:773) | squish on joint right_j1 - 20.110 > 20.000

These collision information can also be viewed in rqt_console. The images below show the impact and squish logged in rqt_console.

Impact.png

Squish.png


To disable the collision detection (squish and impact), publish an empty message at greater than 5 Hz to the topic /robot/limb/right/suppress_contact_safety. The squish and impact detection will stay suppressed as long as this is being published.

For eg, the command to suppress collision detection for Sawyer would be:

$ rostopic pub /robot/limb/right/suppress_contact_safety std_msgs/Empty -r 10

Gravity Compensation Torques

In order to oppose the effect of gravitational force acting across Sawyer's arm, gravity compensation torques have to be applied across that arm. The built-in gravity compensating model uses KDL for calculating the gravity compensation torques. In addition, inertial feed-forward and interaction torques are calculated depending on which controller is active. These torques are applied before sending joint commands to the Joint Control Boards.

The gravity compensation torques are available via the topic robot/limb/right/gravity_compensation_torques of message type intera_core_msgs/SEAJointState

$ rostopic echo robot/limb/right/gravity_compensation_torques

In order to disable the gravity compensation torques, an empty message should be published to the topic /robot/limb/right/suppress_gravity_compensation at greater than 5 Hz.

$ rostopic pub -r 10 /robot/limb/right/suppress_gravity_compensation std_msgs/Empty


The fields of the Gravity Compensation Torques message:

commanded_position: Commanded Position from RealTime Loop

commanded_velocity: Commanded Velocity from RealTime Loop

commanded_effort: Commanded Effort from RealTime Loop. In torque control mode this will be the torques you commanded. 
                  In position control mode this is the integral effort applied achieving the goal positions.

actual_position: Measured Position from RealTime Loop

actual_velocity: Measured Velocity from RealTime Loop

actual_effort: Measured Effort from RealTime Loop via the Spring Deflection Sensors in each Series Elastic Actuator

gravity_model_effort: Gravity Compensation Torques Commanded (includes the inertial feed forward torques when applicable)

gravity_only: Torque required to hold the arm against gravity returned by KDL if the arm was stationary

interaction_torque: Torque produced by the Interaction Controller plugin

hysteresis_model_effort: Spring Compensation Torques Commanded. [Not applicable to Sawyer]

crosstalk_model_effort: Joint Crosstalk Compensation. [Not applicable to Sawyer]

hystState: A variable used in calculating the hysteresis model. [Not applicable to Sawyer]

Zero-G mode

Zero-G mode allows the user to free move the arm. In this mode, the joint position controllers are disabled while the gravity compensation torques are still active to reduce the gravitational forces on the arm.

The Zero-G mode can be enabled by grasping the cuff over its groove or pressing the circle button on the navigators,

Sawyer Training Cuff.png SDK Navigator.png

Note: The estimated gravitational torques depends on the accuracy on the arm's kinematic model, link mass properties, and torque sensing. Because the estimated torques are not perfect, the arm may drift if the cuff is not firmly held while zero-g mode is active. If there is a strong pull, try running the arm calibration routine.

Unlike the gravity compensation model, the Zero-G mode cannot be directly disabled. The cuff interaction has to be disabled in order to disable the Zero-G. Publishing an empty message to the topic /robot/limb/right/suppress_cuff_interaction at greater than 5Hz, disabled the cuff interaction.

rostopic pub -r 10 /robot/limb/right/suppress_cuff_interaction std_msgs/Empty