Endpoint Impedance Control for Sawyer (Intera 5.2)

[sdasari]

We're trying to use the new intera 5.2 software to do endpoint impedance control on the Sawyer robot. The primary goal is to enable unsupervised data collection for machine learning research using the Sawyer arm, which naturally requires reasonably precise yet "soft" control (rather not apply too much force to avoid breaking objects).

I've seen the interaction control tutorial and tried some of the example scripts. The demo scripts has two problems that I'm not sure how to get past:

1. Only works wells when waypoints are far apart. Controller seems somewhat unstable when the waypoints are super close together.
2. Has super stiff control to get to a target location and then switches to a softer controller. We'd rather have the smooth controller engaged throughout the entire trajectory, in order to get a safe/smooth motion from start to target position. 

We've actually been using a modified version of the joint_torque_springs tutorial from earlier intera versions. Is there a better (e.g more precise/safer/easier) way to do this with the new interaction interface?

[Ian McMahon]

Hi Sudeep,

Apologies for the lack of response here. I need a bit more input from our Controls Roboticists to properly answer your question. I will touch base with them Monday and get back to you. Thank you for your patience.

Best,
~ Ian

[Andy Park]

Hi Sudeep,

 

I am glad that you are trying to use our new interaction control interface for your cool project. We worked hard to get those features out. Hope that they will work out great for your research. 

To better assist you, would you be able to better clarify your questions?

 

The demo scripts has two problems that I'm not sure how to get past:

Which demo scripts are you referring to? In interaction control tutorial, we show the usages of three examples scripts (go_to_joint_angles_in_contact.py, set_interaction_options.py, and constrained_zeroG.py). I wonder if the problems exist in all those scripts or only one of them. 

 

1. Only works wells when waypoints are far apart. Controller seems somewhat unstable when the waypoints are super close together. 

Can you clarify what you mean by waypoints being close or far apart? Do you mean that when the joint command positions are more tightly sampled and sent to the robot, the robot goes unstable? What do you mean by the robot being unstable? How did it behave? Can you show me a video if it is not easy to describe? 

 

2. Has super stiff control to get to a target location and then switches to a softer controller. We'd rather have the smooth controller engaged throughout the entire trajectory, in order to get a safe/smooth motion from start to target position.

Can you clarify what you mean by controllers being stiff or soft? We have an interaction controller that can be set to one of four modes (as described in the wiki page), and in impedance mode, the robot will behave based on the impedance (stiffness) parameters that are specified by the user (if unspecified, it will use the default stiffness parameters which are in fact maximum impedance in all directions). So, how soft or stiff it is totally based on those parameters, which means that they are under your control. I wonder if you are confused with our collision behavior, which is called squish. Squish is triggered when arm sees unexpected force/torque (larger than some thresholds we specify), and arm will yield for safety. 

 

The previous code has actually been using a modified version of the joint_torque_springs (https://github.com/RethinkRobotics/intera_sdk/blob/master/intera_examples/scripts/joint_torque_springs.py) tutorial from earlier intera versions. Is there a better (e.g more precise/safer/easier) way to do this with the new interaction interface?

Would you be able to share with us what exactly you tried? joint_torque_springs.py is a joint stiffness control example, which has existed since Baxter. The interaction interface is for endpoint force/impedance control, so joint torque springs example cannot be replicated using interaction interface unless you are interested in joint stiffness control in nullspace only, which you can do with interaction interface. Our interaction control interface can command endpoint stiffnesses upto 3000N/m and 300N/rad (with max impedance enabled), so it can be quite precise. And our collision behavior that I mentioned provides safety mechanism for unexpected contacts during the interaction with external environment/objects. So I believe that you should be able to achieve your goal with what we provide in the SDK. Would you be able to also clarify what the difficulties were in using those scripts? 

 

Hope to hear from you soon. 

 

Best,

-- Andy

[jscholz]

Hello,

I'm also interested in this question, for a similar application.  

I've gone through the documentation, but it is not clear to me what control commands are supported while in interaction mode.  Can you send raw joint positions, velocities, or torques?  Can you send cartesian velocities or positions?  

My current impression is that interaction_control is designed to work with the MotionTrajectory message, but not necessarily JointCommand messages on the '/robot/limb/right/joint_command' topic. 

I've also observed confusing behavior when trying to control the arm in interaction_mode.  

Here's what I've tried:

1) start interaction control mode using the demo script:

`$ rosrun intera_examples set_interaction_options.py -k 1000.0 1000.0 1000.0 1000.0 1000.0 1000.0 -m 0 0 0 0 0 0`

2) (in another tab) send joint position commands to the arm

 

[jscholz]

[accidentally submited before i finished]

...

2) (in another tab) send joint position commands to the arm:

`$ rosrun intera_examples joint_position_keyboard.py`
 

OR

3) (in another tab) send cartesian poses using demo script:

`$ rosrun intera_examples go_to_cartesian_pose.py -R 0.0 0.0 0.03 0. 0. 0. --linear_speed 0.1 --linear_accel 0.1 --rotational_speed 0.2 --rotational_accel 0.2`

Results:

[#2] I've found that when I do #2 with interaction mode ON the arm is very jerky when increasing the joint angle, but when decreasing it is slow and does not return to the same state.  Ie qpos + delta - delta != qpos_init.  If I turn off interaction_mode using `rosrun intera_examples set_interaction_options.py -s 0` the script in #2 works perfectly as expected.  With interaction_mode on it does seem to be compliant while sending these commands, which is what I want.  However, I cannot use this mode if it doesn't behave consistently with joint-pos references.  

[#3] I observed similarly peculiar behavior when sending cartesian poses.  The command in #3 generates nice smooth cartesian movements when interaction_mode is OFF.  However, if I run the exact same command while interaction_mode is ON the arm lurches abruptly every time I update the pose.  This is the opposite of what I expected, and is clearly undesirable for a safety controller.

I'm otherwise very please with the impedance mode in 5.2 for a fixed pose, and would love to be able to use it for our applications.  Any guidance on the intended way to update the joint or cartesian refs for this controller are much appreciated.

Thanks in advance!

[Andy Park]

Hi jscholz,

I am so glad to hear that you are happy with the interaction control behavior with a fixed pose. I am sorry that the current documentation did not make it clear to you regarding how to command impedance behavior during motion. Before answering your questions, I want to let you know that the other scripts that you ran with set_interaction_options scripts (joint_position_keyboard  and go_to_cartesian_pose) were not written to be compatible with interaction mode. A better to script for you to try out is go_to_joint_angles_in_contact script. It will show how to run motion in interaction mode through intera motion interface. That script is essentially a combination of set_interaction_options (see here) and go_to_joint_angles (see here) scripts. Following this example, you should be able to create another script using go_to_cartesian_pose as well (e.g., go_to_cartesian_pose_in_contact).

To answer your questions, the reason that joint_position_keyboard.py did not bring the arm to the same state is that in interaction controller arm is controlled via endpoint impedance control along with nullspace stiffness control. In doing endpoint impedance control, the endpoint reference pose is computed from joint command positions. That being said, although joint command positions set back to the initial ones, it is not guaranteed that all joints move exactly to the initial positions if arm is in interaction mode. So, qpos + delta - delta != qpos_init is quite expected although the difference should be minimal with large endpoint and nullspace stiffness values. 

In fact, the right way to command endpoint/joint motion in interaction mode is to command joint positions directly to /robot/limb/right/joint_command and as long as you are commanding a smooth motion trajectory, this should give a desired behavior. My suggested workflow would be as follows:

1) In the first terminal, set desired interaction parameters using set_interaction_options script.

2) open a second terminal, and use your custom script to send desired joint command positions. It is important to command joint positions consistently at a high fixed rate (500-800Hz) so that the motion will not be jerky. You can set the mode for jointComand message into POSITION_MODE. You can test this script in position mode (without running set_interaction_options) and see how the motion looks before trying along with 1). 

Note that currently we do not support directly commanding endpoint pose during interaction mode. It is because desired nullspace configurations as well as desired endpoint pose can be obtained from joint command positions. This means that although you want to generate endpoint motion from your reference endpoint trajectory, you always need to use IK first to compute joint command positions. I will refer you to our IK service if you would like to learn how to compute joint positions from endpoint pose. 

Hope these explanations helped answering your questions. Please feel free to add more comments here (or create a new question) if you have any more questions. I am sorry that our current documentation of interaction interface did not make these points clear to you. In our next iteration of updating the documentation, we will try to add some Q&A to the interface in order to capture these points for other users.  I really hope that you guys will be able to find our interaction interface useful for your cool research projects, and your thoughts or feedback will be greatly appreciated for our next update. 

Best,

-- Andy

 

 

[sdasari]

Hi Andy,

My original question was actually based off the go_to_joint_angles_in_contact script. It seems like that script wasn't working for the reasons I listed above (really stiff movement before switching into interaction control, and waypoints set in intera_interface api have to be far apart). I did play with the input parameters but it didn't seem to have an effect on the scripts behavior. It's very possible I'm missing something here, so please let me know if that's the case.

The second suggestions you gave did help much more - however! I wrote a script that linearly interpolates between start and end joint angles (usually calculated w/ IK service), and sends a POSITION_MODE JointCommand to /robot/limb/right/joint_command at ~800 Hz. In the background I had the set_interaction_options script running. It generates smooth motion without interaction mode (aka when script not running), and I can adjust the controller's stiffness when interaction mode is running. When interaction control is on, the motion is definitely safer/smoother for our purposes.

Unfortunately, I'm having issues ensuring the trajectory is smooth in interaction_mode: sometimes longer motions are jerky even when I interpolate at 800 Hz. Do you have any recommendations for adjusting the interaction control stiffness/my interpolation code to ensure that the trajectory is smooth/safe as possible? In other words, what is the best way to "command joint positions consistently at a high fixed rate (500-800Hz)", when interaction control mode is on.

Thanks for the response, by the way! Interaction mode looks really handy ☺️

 

[Ian McMahon]

Hi Sudeep,

Addressing your second question, Position mode is probably not the suitable control mode for your usecase, as it controls the speed of each joint based on the distance to the current joint angle furthest from the commanded joint position. A much more straight-forward command mode is Trajectory Mode: in Trajectory mode, you will supply a vector of joint [Position, Velocity, Acceleration] for every joint on the arm. The robot will attempt to achieve these commands as you supply them to the robot, without trying to interpolate any joint speeds. This requires a bit more work on your end to properly calculate the Position ,Velocity and Acceleration commands to be smooth and consistent, but those commands are converted directly to Torques and passed into the Joint Control Boards. Keep in mind, if the Velocities or Accelerations supplied in this control mode are not consistent between timesteps, or are delivered at an inconsistent time interval, this can produce less than smooth motion.

To determine if this control mode makes sense, I would recommend trying Trajectory Mode side-by-side with Position mode, without Interaction Control parameters supplied to the robot to determine which produces smoother trajectories for your application. Then you can add back in the Interaction Control mode parameters. I hope this helps!

Best,
~ Ian

[sdasari]

Hi Ian,

Thanks for the response. Do you have any tutorials or resources for using Trajectory mode? I assume I should be sending the same JointCommand but w/ the whole position/velocity/acceleration vector and in Trajectory mode.

Other than that I unfortunately don't know how to go about calculating Velocity/Acceleration so they work well with your controller. Is there a python/ros package here that I'm missing?

 

Sudeep

[Ian McMahon]

Hi Sudeep,

The SDK you have already installed has the functionality you need to command Trajectory Mode:

Class - Limb Interface Class:
https://github.com/RethinkRobotics/intera_sdk/blob/master/intera_interface/src/intera_interface/limb.py#L425-L453
Example Usage - Joint Trajectory Action Server:
https://github.com/RethinkRobotics/intera_sdk/blob/master/intera_interface/src/intera_joint_trajectory_action/joint_trajectory_action.py#L243-L247

As for calculating consistent Position, Velocity, and Acceleration values for each joint, I would recommend avoiding discontinuities in all three, while remaining within the URDF limits. You could also artificially limit the velocity and acceleration limits to make the robot a bit slower than the URDF limits. Please see some example values of these lower limits in the Sawyer MoveIt package. As for how to calculate the values, there are many methods including cubic splining techniques, or differentiating and filtering the joint position commands over the delta time steps for velocity, and then again for acceleration. I don't have any examples on this exactly, but you can use the techniques found in Robotics textbooks to generate these values. A quick search on google turned up a useful set of notes from a class at Columbia on Trajectory planning:
http://www.cs.columbia.edu/~allen/F15/NOTES/trajectory.pdf

That's not an exhaustive resource, but it should hopefully help you get started. I hope this helps!

~ Ian

[Andy Park]

On 7/9/2018 at 5:29 PM, sdasari said:

My original question was actually based off the go_to_joint_angles_in_contact script. It seems like that script wasn't working for the reasons I listed above (really stiff movement before switching into interaction control, and waypoints set in intera_interface api have to be far apart). I did play with the input parameters but it didn't seem to have an effect on the scripts behavior. It's very possible I'm missing something here, so please let me know if that's the case.

Hi Sudeep,

I am glad to hear that you found interaction mode handy.  We will try to improve it further in the next release based on users' feedback like yours. Thank you!

I tried to reproduce the behaviors you described but I could not. Really stiff movement may indicate that you were in position mode before running the script. As long as the motion is running, arm should switch to interaction mode and it should be always compliant. I did not observe any instability issue that you mentioned either. Feel free to open go_to_joint_angles_in_contact script and modify it until you see desired behavior although I do not have any suggestions for you right now. I think the script is quite simple and clear since it is just a combination of go_to_joint_angles and set_interaction_options scripts. Nevertheless, please feel free to show us any videos if you see any unstable/undesired behaviors on the robot. 

Cheers,

-- Andy

[Guru]

Hi Rethink and others!

First off, the impedance control released in intera 5.2 is great performance wise!

I think this post is relevant to this topic. I would like to set the tool center point for interaction control. Any help would be awesome.

P.S. I also found that trying the demo joint_position_keyboard.py along with interaction control caused jerky behavior. I didn't play around with it too much because my current application doesn't require real-time control however in the future this would be necessary.

Maybe a simple interface which allows commanding the robot position and 6 axis impedance/force all at the same time would be useful instead of two different messages. If the controller could account for the absence of velocity and acceleration messages and prevent jerky behavior, that would be great too.

Thanks!

Guru

[jscholz]

This has been a super useful thread so far, thanks all!
I have two follow-ups which I also think are related.  

First, I implemented the same approach as @sdasari of using joint position commands, and also observed jerky behavior.  However my use case is slightly different in that I don't have trajectories, and instead my commands are generated every timestep by a neural network.  @Ian McMahon suggested using Trajectory mode, but I'm not sure how that would work for a single "waypoint" at a time.  What I really want (I think) is a way to send joint-velocity commands through the interaction controller.  However, when I enable interaction_mode the robot is only responsive to JointCommand messages with positions, not velocities.  Is there a way to send joint-velocities while the impedance controller is running?

 

Second, I can't figure out what the interaction_frame argument does in interaction_mode.  I've played with all sorts of different values expecting there to be an offset in the origin of rotation when executing this command:

`$ rosrun intera_examples set_interaction_options.py -k 0.0 0.0 0.0 0.0 0.0 0.0 -m 1 1 1 0 0 0 -fr 0.1 0.2 0.3 1 0 0 0`

However it always seemed to freely pivot around the same point in the wrist.  Can you clarify what this frame argument in the message does?  Is it redundant with in_endpoint_frame?

[jscholz]

Edit: Ignore my second question above, hadn't seen reply to Guru's separate post.  

[sdasari]

Hi all,

@Andy Park the instabilities I noticed occurred when the robot was commanded to go to positions close to each other when I used the waypoint interface exposed in go_to_cartesian_pose. I wasn't doing any smoothing/interpolation myself at that point which may have caused the jerkiness. It'd be great if you could get that waypoint interface to do the smoothing for us: in effect just let us send poses and it handles all the interpolation/movement.

 

@jscholz I was able to get the impedance control to work by manually interpolating between the target poses using a Cartesian spline. At that point position_mode worked great. Here's some sample code that worked well for me along with

rosrun intera_examples set_interaction_options.py -r 10 -k 0.05 0.05 1000 10 10 10 -m 1 1 0 1 1 1

Our network doesn't predict at a very high rate so this sort of scheme works great for us. Please do send any comments you have for this sort of scheme. I definitely think it can be improved.

#!/usr/bin/env python

import numpy as np
import rospy
from std_msgs.msg import Empty
from sensor_msgs.msg import JointState
from scipy.interpolate import CubicSpline
import intera_interface
from intera_interface import CHECK_VERSION
from std_msgs.msg import Float32
from std_msgs.msg import Int64
from intera_core_msgs.msg import JointCommand
from threading import Lock

class CSpline:
    def __init__(self, points, duration=1., bc_type='clamped'):
        n_points = points.shape[0]
        self._duration = duration
        self._cs = CubicSpline(np.linspace(0, duration, n_points), points, bc_type=bc_type)

    def get(self, t):
        t = np.array(min(t, self._duration))

        return self._cs(t), self._cs(t, nu=1), self._cs(t, nu=2)

NEUTRAL_JOINT_ANGLES = np.array([0.412271, -0.434908, -1.198768, 1.795462, 1.160788, 1.107675, 2.068076])
max_vel_mag = np.array([0.88, 0.678, 0.996, 0.996, 1.776, 1.776, 2.316])
max_accel_mag = np.array([3.5, 2.5, 5, 5, 5, 5, 5])

class ImpedanceController(object):
    """
    BEFORE RUNNING THIS NODE IN A SEPERATE TERMINAL RUN:
    rosrun intera_examples set_interaction_options.py -k [IMPEDANCE STIFFNESS] -r 10
        - run "rosrun intera_examples set_interaction_option.py" -h for details
    """
    def __init__(self, limb = "right"):
        rospy.init_node("custom_impedance_controller")
        rospy.on_shutdown(self.clean_shutdown)

        self._rs = intera_interface.RobotEnable(CHECK_VERSION)
        self._init_state = self._rs.state().enabled
        self._rs.enable()

        if not self._rs.state().enabled:
            raise RuntimeError("Robot did not enable...")

        self._limb = intera_interface.Limb(limb)
        print("Robot enabled...")

        # control parameters
        self._rate = rospy.Rate(800)  # Hz
        self._j_names = [n for n in self._limb.joint_names()]
        self._duration = 1.5
        self._desired_joint_pos = NEUTRAL_JOINT_ANGLES.copy()
        self._fit_interp()

        #synchronizes commands
        self._global_lock = Lock()

        # create cuff disable publisher
        cuff_ns = 'robot/limb/' + limb + '/suppress_cuff_interaction'
        self._pub_cuff_disable = rospy.Publisher(cuff_ns, Empty, queue_size=1)
        self._cmd_publisher = rospy.Publisher('/robot/limb/{}/joint_command'.format(limb), JointCommand, queue_size=100)

        print("Running. Ctrl-c to quit")

        rospy.Subscriber("desired_joint_pos", JointState, self._set_des_pos)
        rospy.Subscriber("interp_duration", Float32, self._set_duration)
        rospy.Subscriber("imp_ctrl_active", Int64, self._imp_ctrl_active)

        self._ctrl_active = True
        self._node_running = True

        self._rate.sleep()
        self._start_time = None
        while self._node_running:
            if self._ctrl_active:
                self._pub_cuff_disable.publish()
                self._global_lock.acquire()

                if self._start_time is None:
                    self._start_time = rospy.get_time()
                elif self._start_time + self._duration < rospy.get_time():
                    self._fit_interp()
                    self._start_time = rospy.get_time()

                t = min(rospy.get_time() - self._start_time, self._duration)   #T \in [0, self._duration]
                pos, velocity, accel = self._interp.get(t)

                command = JointCommand()
                command.mode = JointCommand.POSITION_MODE
                command.names = self._j_names
                command.position = pos
                # command.velocity = np.clip(velocity, -max_vel_mag, max_vel_mag)
                # command.acceleration = np.clip(accel, -max_accel_mag, max_accel_mag)
                self._cmd_publisher.publish(command)
                self._global_lock.release()

            self._rate.sleep()

    def _get_joints(self):
        return np.array([self._limb.joint_angle(n) for n in self._j_names])

    def _imp_ctrl_active(self, inp):
        if inp.data == 1:
            print('impedance ctrl activated')
            self._ctrl_active = True
        if inp.data == 0:
            print('impedance ctrl deactivated')
            self._ctrl_active = False

    def _fit_interp(self):
        self._interp = CSpline(self._get_joints(), self._desired_joint_pos.copy(), self._duration)

    def _set_des_pos(self, jointstate):
        des_angles = dict(list(zip(jointstate.name, jointstate.position)))
        self._desired_joint_pos = np.array(des_angles[n] for n in self._j_names)

        self._global_lock.acquire()
        self._fit_interp()
        self._start_time = None
        self._global_lock.release()

    def _set_duration(self, duration):
        duration = duration.data
        if duration <= 0:
            print("DURATION CANNOT BE NEGATIVE!!")
            return

        print("setting duration to", duration)

        self._global_lock.acquire()
        self._duration = float(duration)
        self._global_lock.release()




    def clean_shutdown(self):
        """
        Switches out of joint torque mode to exit cleanly
        """
        self._node_running = False
        print("\nExiting example...")
        self._limb.exit_control_mode()
        if not self._init_state and self._rs.state().enabled:
            print("Disabling robot...")
            self._rs.disable()




if __name__ == "__main__":
ImpedanceController()

 

[Andy Park]

That is awesome! Thanks for sharing the code as well. I will look into the script more later and let you know if I find any ways for further improvement.