- 1 Control Overview
- 2 Control Modes
- 3 Motion Controller Interface
- 4 Interaction Control Mode
- 5 Joint Trajectory Action Server
- 6 Collision Model
- 7 Gravity Compensation Torques
- 8 Zero-G mode
Joint Command Overview
Sawyer's arm controls 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 three joint control modes for the user to choose from at the top level: position, 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.
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
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
RealTime Motor Controller
The RealTime Motor Control loop on the robot 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 and Detection, Gravity Compensation and Spring Hysteresis/Crosstalk 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.
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:
at a rate > 5hz (by default). The rate at which you must publish your messages is defined on the topic:
of message type std_msgs/Float64
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
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 [
command is the ordered (corresponding to the
names field) command values.
names is a list of strings containing the joints to command.
Joint Control Fundamentals
When a joint position command is published from the development PC, the '
realtime_loop' process which represents a motor control plugin subscribes to this message. This message is then parsed, and represented in memory based on control mode.
Depending on the control mode, modifications are 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. This states that if a new '
JointCommand' message is not received within the specified timeout (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. The reason for this is safety. For example, if controlling in velocity control mode where you are commanding 1.0 Rad/s to joint j0, and you lose network connectivity (someone kicks your router), the robot could result in dangerous motions. By 'timing out', the robot will be safer, reacting to network timeouts, or incorrect control behavior.
This control rate timeout is configurable:
of message type std_msgs/Float64
Joint Position Control
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.
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 Trajectory-Position Control
Joint Trajectory-Position Control mode was introduced to provide a much more direct position control method. 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.
The Joint Trajectory-Position commands only perform the following modification (typically these additions are none):
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.
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 values, a commanded velocity for each of the seven joints.
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:
Note: When commanding joint velocities, if a commanded velocity to one of the joints will result in a joint position that is beyond the joints limits, no joints will be commanded, as all of the command is considered invalid. Reason we do this; an example of a common control method, using the Jacobian for Cartesian control resulting in joint velocity commands. If a single joint hits its limit, the rest of the joints will still be commanded, resulting in obscure and potentially dangerous motions. We recommend either implementing a joint space potential field or joint limit 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 at 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.
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, taking special note of the absence of collision avoidance and detection modules:
Note: As shown in the above image, joint torque commands are applied in addition to gravity and spring compensation 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.
Motion Controller Interface
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
intera_motion_msgs/Trajectory 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.
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. 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 (prefered method) or the
MotionControllerActionClient class (advanced users).
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
InteractionOptions classes are all utilities that help with the construction of trajectories.
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
MotionWaypointOptions class is used as utility to create
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.
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!)
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 Joint Trajectory Action. To jump straight to the source code, see the intera_interface JTAS page on GitHub.
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:
rosrun intera_interface joint_trajectory_action_server.py
- Velocity Mode (
- Position Mode (
- Inverse Dynamics Feed Forward Position Mode (default 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.
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.
Inverse Dynamics Feed Forward Position Mode (using Join Trajectory Position Mode)
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
/robot/inverse_dynamics topic. MoveIt! supplies these three dimensions by default, so it is highly recommended that you use this mode when using that motion planner.
There are a total of three levels of collision models in Sawyer.
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.
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.
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
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.
To disable the collision avoidance on a right arm, you need to publish an empty message at greater than 5hz to the topic,
For eg, the command to suppress collision avoidance on the right arm would be:
$ rostopic pub /robot/limb/right/suppress_collision_avoidance std_msgs/Empty -r 10
The collision avoidance will stay suppressed as long as this is being published
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 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.
The gravity compensation torques are available via the topic
robot/limb/right/gravity_compensation_torques of message type
$ rostopic echo robot/limb/right/gravity_compensation_torques # of message type intera_core_msgs/SEAJointState
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 hysteresis_model_effort: Spring Compensation Torques Commanded. This is the hysterisis effort applied to account for spring joint j1 crosstalk_model_effort: Joint Crosstalk Compensation. Present here is minimal compensation for joint j1 crosstalk caused by the arm's downstream joints hystState: A variable used in calculating the hysteresis model.
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.
The Zero-G mode can be enabled by grasping the cuff over its groove or press circle button on navigators,
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