arm_server.py

Go to the documentation of this file.

#!/usr/bin/python
#
# Copyright (c) 2009, Georgia Tech Research Corporation
# All rights reserved.
# 
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions are met:
#     * Redistributions of source code must retain the above copyright
#       notice, this list of conditions and the following disclaimer.
#     * Redistributions in binary form must reproduce the above copyright
#       notice, this list of conditions and the following disclaimer in the
#       documentation and/or other materials provided with the distribution.
#     * Neither the name of the Georgia Tech Research Corporation nor the
#       names of its contributors may be used to endorse or promote products
#       derived from this software without specific prior written permission.
# 
# THIS SOFTWARE IS PROVIDED BY GEORGIA TECH RESEARCH CORPORATION ''AS IS'' AND
# ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
# WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
# DISCLAIMED. IN NO EVENT SHALL GEORGIA TECH BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
# LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA,
# OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
# LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE
# OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
# ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#

# Author: Advait Jain

# moved this to the top to get PyKDL from the ROS package for the Meka
# modules as well.
import roslib; roslib.load_manifest('hrl_cody_arms')
roslib.load_manifest('force_torque') # Advait hack on Oct 25, 2011

import force_torque.FTClient as ftc


import m3.rt_proxy as m3p
import m3.arm
import m3.toolbox as m3t
import m3.pwr as m3w
import m3.loadx6

import m3.component_factory as m3f

import cody_arm_client as ac

import numpy as np, math
import sys, time, os
import copy
from threading import RLock

import rospy

import hrl_lib.transforms as tr
from hrl_msgs.msg import FloatArray
from std_msgs.msg import Header, Bool, Empty

from sensor_msgs.msg import JointState

THETA_GC = 5
TORQUE_GC = 4
THETA = 3
OFF = 0 # in OFF mode also, the behavior is strange. Not the same as hitting the estop (Advait, Jan 1 2010)


## 1D kalman filter update.
def kalman_update(xhat, P, Q, R, z):
    #time update
    xhatminus = xhat
    Pminus = P + Q
    #measurement update
    K = Pminus / (Pminus + R)
    xhat = xhatminus + K * (z-xhatminus)
    P = (1-K) * Pminus
    return xhat, P

class MekaArmSettings():
    def __init__(self, stiffness_list=[0.7,0.7,0.8,0.8,0.3],
                 control_mode='theta_gc'):
        ''' stiffness_list: list of 5 stiffness values for joints 0-4.
            control_mode: 'theta_gc' or 'torque_gc'
        '''
        self.set_stiffness_scale(stiffness_list)
        self.control_mode = control_mode

    def set_stiffness_scale(self, l):
        # for safety of wrist roll. Advait Jun 18, 2010.
        # changed to 0.2 from 0.3 (Advait, Sept 19, 2010)
        l[4] = min(l[4], 0.2)
        self.stiffness_list = l


class MekaArmServer():
    def __init__(self, right_arm_settings=None,
                 left_arm_settings=None, use_netft_r = False,
                 use_netft_l = False, enable_right_arm=True,
                 enable_left_arm=True):
        self.arm_settings = {}  # dict is set in set_arm_settings

        self.left_arm_ft = {'force': np.matrix(np.zeros((3,1),dtype='float32')),
                            'torque': np.matrix(np.zeros((3,1),dtype='float32'))}
        self.right_arm_ft = {'force': np.matrix(np.zeros((3,1),dtype='float32')),
                             'torque': np.matrix(np.zeros((3,1),dtype='float32'))}

        # kalman filtering force vector. (self.step and bias_wrist_ft)
        self.Q_force, self.R_force = {}, {}
        self.xhat_force, self.P_force = {}, {}

        self.Q_force['right_arm'] = [1e-3, 1e-3, 1e-3, 1e-3, 1e-3, 1e-3]
        self.R_force['right_arm'] = [0.1**2, 0.1**2, 0.1**2, 0.05**2, 0.05**2, 0.05**2]
        self.xhat_force['right_arm'] = [0., 0., 0., 0., 0., 0.]
        self.P_force['right_arm'] = [1.0, 1.0, 1.0, 0., 0., 0.]

        self.Q_force['left_arm'] = [1e-3, 1e-3, 1e-3, 1e-3, 1e-3, 1e-3]
        self.R_force['left_arm'] = [0.1**2, 0.1**2, 0.1**2, 0.05**2, 0.05**2, 0.05**2]
        self.xhat_force['left_arm'] = [0., 0., 0., 0., 0., 0.]
        self.P_force['left_arm'] = [1.0, 1.0, 1.0, 0., 0., 0.]

        self.enable_left_arm = enable_left_arm
        self.enable_right_arm = enable_right_arm

        if not enable_right_arm:
            right_arm_settings = None
            self.right_arm_ft = None
            self.Q_force['right_arm'] = None
            self.R_force['right_arm'] = None
            self.P_force['right_arm'] = None
            self.xhat_force['right_arm'] = None

        if not enable_left_arm:
            left_arm_settings = None
            self.left_arm_ft = None
            self.Q_force['left_arm'] = None
            self.R_force['left_arm'] = None
            self.P_force['left_arm'] = None
            self.xhat_force['left_arm'] = None

        self.initialize_joints(right_arm_settings, left_arm_settings)

        #self.initialize_gripper()


        #----- ROS interface ---------
        rospy.init_node('arm_server', anonymous=False)

        if use_netft_r:
            self.ftclient_r = ftc.FTClient('force_torque_ft5', True)
            self.ftclient_r.bias()
        else:
            self.ftclient_r = None

        if use_netft_l:
            self.ftclient_l = ftc.FTClient('force_torque_ft6', True)
            self.ftclient_l.bias()
        else:
            self.ftclient_l = None


        self.q_r_pub = rospy.Publisher('/r_arm/q', FloatArray)
        self.q_l_pub = rospy.Publisher('/l_arm/q', FloatArray)

        self.qdot_r_pub = rospy.Publisher('/r_arm/qdot', FloatArray)
        self.qdot_l_pub = rospy.Publisher('/l_arm/qdot', FloatArray)

        self.force_raw_r_pub = rospy.Publisher('/r_arm/force_raw', FloatArray)
        self.force_raw_l_pub = rospy.Publisher('/l_arm/force_raw', FloatArray)
        self.force_r_pub = rospy.Publisher('/r_arm/force', FloatArray)
        self.force_l_pub = rospy.Publisher('/l_arm/force', FloatArray)

        self.jep_r_pub = rospy.Publisher('/r_arm/jep', FloatArray)
        self.jep_l_pub = rospy.Publisher('/l_arm/jep', FloatArray)
        self.alph_r_pub = rospy.Publisher('/r_arm/joint_impedance_scale', FloatArray)
        self.alph_l_pub = rospy.Publisher('/l_arm/joint_impedance_scale', FloatArray)

        self.torque_r_pub = rospy.Publisher('/r_arm/joint_torque', FloatArray)
        self.torque_l_pub = rospy.Publisher('/l_arm/joint_torque', FloatArray)

        self.mot_temp_r_pub = rospy.Publisher('/r_arm/joint_motor_temp', FloatArray)
        self.mot_temp_l_pub = rospy.Publisher('/l_arm/joint_motor_temp', FloatArray)

        self.pwr_state_pub = rospy.Publisher('/arms/pwr_state', Bool)
        self.joint_state_pub = rospy.Publisher('/joint_states', JointState)
        
        rospy.Subscriber('/r_arm/command/jep', FloatArray, self.r_jep_cb)
        rospy.Subscriber('/l_arm/command/jep', FloatArray, self.l_jep_cb)
        rospy.Subscriber('/r_arm/command/joint_impedance_scale', FloatArray, self.r_alpha_cb)
        rospy.Subscriber('/l_arm/command/joint_impedance_scale', FloatArray, self.l_alpha_cb)

        # publishing to this message will stop the arms but also crash
        # the server (since meka server crashes.) Advait Nov 14, 2010
        rospy.Subscriber('/arms/stop', Empty, self.stop)
        rospy.Subscriber('/arms/command/motors_off', Empty,
                         self.motors_off)

        self.cb_lock = RLock()
        self.r_jep = None # see set_jep
        self.l_jep = None # see set_jep
        self.q_r = None # using this to compute qdot
        self.q_l = None # using this to compute qdot
        self.q_rHOT = None # using this to compute qdot # added_by_tapo, Jan 23-2013
        self.q_lHOT = None # using this to compute qdot # added_by_tapo, Jan 23-2013
        self.time_stamp = None # using this to compute qdot

        d = {}
        d['right_arm'] = ac.get_joint_name_list('r')
        d['left_arm'] = ac.get_joint_name_list('l')
        self.joint_names_list = d

    def set_arm_settings(self,right_arm_settings,left_arm_settings):
        self.arm_settings['right_arm'] = right_arm_settings
        self.arm_settings['left_arm'] = left_arm_settings

        for arm,arm_settings in zip(['right_arm','left_arm'],[right_arm_settings,left_arm_settings]):
            joint_component_list = self.joint_list_dict[arm]

# OFF mode doesn't seem to work. (Advait, Jan 1 2010)
            if arm_settings == None:
                if joint_component_list != None:
                    for c in joint_component_list:
                        c.set_control_mode(OFF)
                continue

            stiffness_list = arm_settings.stiffness_list

            if arm_settings.control_mode == 'torque_gc':
                print 'setting control mode to torque_gc'
                for c in joint_component_list:
                    c.set_control_mode(TORQUE_GC)
                    c.set_torque_mNm(0.0)
            elif arm_settings.control_mode == 'theta_gc':
                print 'setting control mode to theta_gc'
                for i in range(5):
                    joint_component_list[i].set_control_mode(THETA_GC)
                    joint_component_list[i].set_stiffness(stiffness_list[i])
                    joint_component_list[i].set_slew_rate_proportion(1.)

                joint_component_list[5].set_control_mode(THETA)
                joint_component_list[5].set_slew_rate_proportion(1.)
                joint_component_list[6].set_control_mode(THETA)
                joint_component_list[6].set_slew_rate_proportion(1.)

            elif arm_settings.control_mode == 'wrist_theta_gc':
                print 'setting control mode to theta_gc include wrist joints'
                for i in range(7):
                    joint_component_list[i].set_control_mode(THETA_GC)
                    joint_component_list[i].set_stiffness(stiffness_list[i])
                    joint_component_list[i].set_slew_rate_proportion(1.)

            else:
                print 'hrl_robot.initialize_joints. unknown control mode for ', arm,':', arm_settings.control_mode

    # put a few things into safeop so that individual joint
    # components work.
    def safeop_things(self):
        robot_name = 'm3humanoid_bimanual_mr1'
        chain_names = ['m3arm_ma1', 'm3arm_ma2']
        dynamatics_nms = ['m3dynamatics_ma1', 'm3dynamatics_ma2']

        self.proxy.make_safe_operational(robot_name)
        for c in chain_names:
            self.proxy.make_safe_operational(c)
        for d in dynamatics_nms:
            self.proxy.make_safe_operational(d)

    def initialize_joints(self, right_arm_settings, left_arm_settings):
        self.proxy = m3p.M3RtProxy()
        self.proxy.start(True, True) # the second true enables ROS (needed for the skin patch)

        l1 = ['m3pwr_pwr003']

        if self.enable_right_arm:
            l1.append('m3loadx6_ma1_l0')
            l1.append('m3arm_ma1')

        if left_arm_settings is not None:
            l1.append('m3loadx6_ma2_l0')
            l1.append('m3arm_ma2')

        for c in l1:
            if not self.proxy.is_component_available(c):
                raise m3t.M3Exception('Component '+c+' is not available.')
        
        self.joint_list_dict = {}

        if self.enable_right_arm:
            right_l = []
            for c in ['m3joint_ma1_j0','m3joint_ma1_j1','m3joint_ma1_j2',
                      'm3joint_ma1_j3','m3joint_ma1_j4','m3joint_ma1_j5',
                      'm3joint_ma1_j6']:
                if not self.proxy.is_component_available(c):
                    raise m3t.M3Exception('Component '+c+' is not available.')
                right_l.append(m3f.create_component(c))
            self.joint_list_dict['right_arm'] = right_l
        else:
            self.joint_list_dict['right_arm'] = None

        if self.enable_left_arm:
            left_l = []
            for c in ['m3joint_ma2_j0','m3joint_ma2_j1','m3joint_ma2_j2',
                      'm3joint_ma2_j3','m3joint_ma2_j4','m3joint_ma2_j5',
                      'm3joint_ma2_j6']:
                if not self.proxy.is_component_available(c):
                    raise m3t.M3Exception('Component '+c+' is not available.')
                left_l.append(m3f.create_component(c))
            self.joint_list_dict['left_arm'] = left_l
        else:
            self.joint_list_dict['left_arm'] = None

        for arm,arm_settings in zip(['right_arm','left_arm'],[right_arm_settings,left_arm_settings]):
            if arm_settings == None:
                continue
            for comp in self.joint_list_dict[arm]:
                self.proxy.subscribe_status(comp)
                self.proxy.publish_command(comp)

        self.set_arm_settings(right_arm_settings,left_arm_settings)

        self.pwr=m3f.create_component('m3pwr_pwr003')
        self.proxy.subscribe_status(self.pwr)
        self.proxy.publish_command(self.pwr)

        self.arms = {}

        if self.enable_right_arm:
            right_fts=m3.loadx6.M3LoadX6('m3loadx6_ma1_l0')
            self.proxy.subscribe_status(right_fts)

            self.arms['right_arm']=m3.arm.M3Arm('m3arm_ma1')
            self.proxy.subscribe_status(self.arms['right_arm'])
        else:
            right_fts = None
            self.arms['right_arm'] = None

        if self.enable_left_arm:
            left_fts=m3.loadx6.M3LoadX6('m3loadx6_ma2_l0')
            self.proxy.subscribe_status(left_fts)

            self.arms['left_arm']=m3.arm.M3Arm('m3arm_ma2')
            self.proxy.subscribe_status(self.arms['left_arm'])
        else:
            left_fts = None
            self.arms['left_arm'] = None

        self.fts = {'right_arm':right_fts,'left_arm':left_fts}

        self.proxy.step()
        self.proxy.step()

    def initialize_gripper(self):
        #self.right_gripper = m3h.M3Gripper('m3gripper_mg0')
        self.right_gripper = m3h.M3Gripper('m3gripper_mg1')
        self.proxy.publish_command(self.right_gripper)
        self.proxy.subscribe_status(self.right_gripper)

    def step(self):
        self.proxy.step()
        l = []
        if self.enable_right_arm:
            l.append('right_arm')
        if self.enable_left_arm:
            l.append('left_arm')

        for arm in l:
            z = self.get_wrist_force(arm).A1 # Force vector
            #if arm == 'right_arm':
            #    z = self.get_wrist_force_nano().A1

            for i in range(6):
                xhat, p = kalman_update(self.xhat_force[arm][i],
                                        self.P_force[arm][i],
                                        self.Q_force[arm][i],
                                        self.R_force[arm][i], z[i])
                if abs(z[i] - self.xhat_force[arm][i]) > 3.:
                    xhat = z[i] # not filtering step changes.
                self.xhat_force[arm][i] = xhat
                self.P_force[arm][i] = p

    def step_ros(self):
        r_arm = 'right_arm'
        l_arm = 'left_arm'

        self.cb_lock.acquire()

        if self.enable_left_arm:
            l_jep = copy.copy(self.l_jep)
            l_alpha = copy.copy(self.arm_settings['left_arm'].stiffness_list)

        if self.enable_right_arm:
            r_jep = copy.copy(self.r_jep)
            r_alpha = copy.copy(self.arm_settings['right_arm'].stiffness_list)

        self.cb_lock.release()

        if self.enable_left_arm:
            self.set_jep(l_arm, l_jep)
            self.set_alpha(l_arm, l_alpha)

        if self.enable_right_arm:
            self.set_jep(r_arm, r_jep)
            self.set_alpha(r_arm, r_alpha)

        self.step()

        motor_pwr_state = self.is_motor_power_on()

        if not motor_pwr_state:
            self.maintain_configuration()

        if self.enable_right_arm:
            q_r = self.get_joint_angles(r_arm)
            tau_r = self.get_joint_torques(r_arm)
            mot_temp_r = self.get_motor_temps(r_arm)
        else:
            q_r = None
            tau_r = None
            mot_temp_r = None

        if self.enable_left_arm:
            q_l = self.get_joint_angles(l_arm)
            tau_l = self.get_joint_torques(l_arm)
            mot_temp_l = self.get_motor_temps(l_arm)
        else:
            q_l = None
            tau_l = None
            mot_temp_l = None

        # it seems like this estimate of joint velocity should really be filtered
        # or at least a better estimate of the derivative (higher order difference equation)
        # - marc Jul 13, 2012
        # added one more term to achieve a better estimate of the derivative - tapo Jan 23, 2013
        t_now = rospy.get_time()
        if self.q_r != None and self.q_l != None and self.q_rHOT != None and self.q_lHOT != None and self.time_stamp != None:
            dt = t_now - self.time_stamp
            #qdot_r = (np.array(q_r) - np.array(self.q_r)) / dt
            qdot_r = (-np.array(q_r) + 4*np.array(self.q_r) - 3*np.array(self.q_rHOT)) / (2*dt)
            #qdot_l = (np.array(q_l) - np.array(self.q_l)) / dt
            qdot_l = (-np.array(q_l) + 4*np.array(self.q_l) - 3*np.array(self.q_lHOT)) / (2*dt)
        else:
            qdot_r = np.zeros(7)
            qdot_l = np.zeros(7)

        self.q_rHOT = self.q_r
        self.q_lHOT = self.q_l
        self.q_r = q_r
        self.q_l = q_l
        self.time_stamp = t_now

        if self.enable_right_arm:
            f_raw_r = self.get_wrist_force(r_arm).A1.tolist()
            f_r = self.xhat_force[r_arm]

        if self.enable_left_arm:
            f_raw_l = self.get_wrist_force(l_arm).A1.tolist()
            f_l = self.xhat_force[l_arm]

        # publish stuff over ROS.
        time_stamp = rospy.Time.now()
        h = Header()
        h.stamp = time_stamp

        h.frame_id = '/torso_lift_link'
        pos = []
        nms = []

        if self.enable_right_arm:
            self.q_r_pub.publish(FloatArray(h, q_r))
            self.qdot_r_pub.publish(FloatArray(h, qdot_r))
            self.jep_r_pub.publish(FloatArray(h, r_jep))
            self.alph_r_pub.publish(FloatArray(h, r_alpha))
            self.torque_r_pub.publish(FloatArray(h, tau_r))
            self.mot_temp_r_pub.publish(FloatArray(h, mot_temp_r))

            nms = nms + self.joint_names_list['right_arm']
            pos = pos + q_r

            h.frame_id = 'should_not_be_using_this'
            self.force_raw_r_pub.publish(FloatArray(h, f_raw_r))
            self.force_r_pub.publish(FloatArray(h, f_r))
            
        if enable_left_arm:
            self.q_l_pub.publish(FloatArray(h, q_l))
            self.qdot_l_pub.publish(FloatArray(h, qdot_l))
            self.jep_l_pub.publish(FloatArray(h, l_jep))
            self.alph_l_pub.publish(FloatArray(h, l_alpha))
            self.torque_l_pub.publish(FloatArray(h, tau_l))
            self.mot_temp_l_pub.publish(FloatArray(h, mot_temp_l))

            nms = nms + self.joint_names_list['left_arm']
            pos = pos + q_l

            h.frame_id = 'should_not_be_using_this'
            self.force_raw_l_pub.publish(FloatArray(h, f_raw_l))
            self.force_l_pub.publish(FloatArray(h, f_l))

        self.joint_state_pub.publish(JointState(h, nms, pos,
                                    [0.]*len(pos), [0.]*len(pos)))
        self.pwr_state_pub.publish(Bool(motor_pwr_state))

    def is_motor_power_on(self):
        return self.pwr.is_motor_power_on(None)

    # Advait, Aug 8, 2009
    # two steps in motors_on and off because with simply one step
    # pwr.is_motor_on does not get the correct value. (I think this is
    # because at the clock edge when motor on command is sent, the power
    # is still off and thus the status is not affected.)
    def motors_off(self, msg=None):
        self.pwr.set_motor_power_off()

    def motors_on(self):
        self.maintain_configuration()
        self.pwr.set_motor_power_on()
        self.step()
        self.step()

    def maintain_configuration(self):
        l = []
        if self.enable_left_arm:
            l.append('left_arm')
        if self.enable_right_arm:
            l.append('right_arm')

        for arm in l:
            q = self.get_joint_angles(arm)
            if self.arm_settings[arm] == None:
                continue
            if 'theta_gc' not in self.arm_settings[arm].control_mode:
                raise RuntimeError('bad control mode: %s', self.arm_settings[arm].control_mode)
            self.set_jep(arm, q)
            self.cb_lock.acquire()
            if arm == 'right_arm':
                self.r_jep = q
            else:
                self.l_jep = q
            self.cb_lock.release()

    def power_on(self):
        self.maintain_configuration()
        self.proxy.make_operational_all()
        self.safeop_things()
        self.pwr.set_motor_power_on()
        self.step()
        self.step()

    def stop(self, msg=None):
        self.pwr.set_motor_power_off()
        self.step()
        self.proxy.stop()

    ##3X1 numpy matrix of forces measured by the wrist FT sensor.
    #(This is the force that the environment is applying on the wrist)
    # @param arm - 'left_arm' or 'right_arm'
    # @return in SI units
    #coord frame - tool tip coord frame (parallel to the base frame in the home position)
    # 2010/2/5 Advait, Aaron King, Tiffany verified that coordinate frame 
    # from Meka is the left-hand coordinate frame.
    def get_wrist_force(self, arm):
        if arm == 'right_arm' and self.ftclient_r != None:
            return self.get_wrist_force_netft('r')

        if arm == 'left_arm' and self.ftclient_l != None:
            return self.get_wrist_force_netft('l')

        m = []
        lc = self.fts[arm]
        m.append(lc.get_Fx_mN()/1000.)
        m.append(lc.get_Fy_mN()/1000.)
        m.append(-lc.get_Fz_mN()/1000.)        

        m.append(lc.get_Tx_mNm()/1000.)
        m.append(lc.get_Ty_mNm()/1000.)
        m.append(-lc.get_Tz_mNm()/1000.)        

        m = np.matrix(m).T
        r = tr.Rz(math.radians(-30.0))

        m1 = r * m[0:3,:]
        m1[1,0] = -m1[1,0]
        m1[2,0] = -m1[2,0]

        m2 = r * m[3:,:]
        m2[1,0] = -m2[1,0]
        m2[2,0] = -m2[2,0]

        return np.row_stack((m1, m2))

    def get_wrist_force_netft(self, arm):
        if arm == 'r':
            w = self.ftclient_r.read()
        elif arm == 'l':
            w = self.ftclient_l.read()

        r = tr.Rz(math.radians(30.))
        f = r * w[0:3]
        t = r * w[0:3]
        f[1,0] = f[1,0] * -1
        t[1,0] = t[1,0] * -1
        return np.row_stack((f,t))

    def get_wrist_torque(self, arm):
        m = []
        lc = self.fts[arm]
        m = tr.Rz(math.radians(-30.0))*np.matrix(m).T
        m[1,0] = -m[1,0]
        m[2,0] = -m[2,0]
        return m



    #-------------------- getting and setting joint angles ------------

    ##
    # @param arm - 'left_arm' or 'right_arm'
    # @return list of 7 joint accelerations in RADIANS/s^2.
    #         according to meka's coordinate frames.
    def get_joint_accelerations(self, arm):
        #return self.arms[arm].get_thetadotdot_rad().tolist()
        raise RuntimeError('The meka joint accelerations are incorrect (Because the joint velocities are incorrect - Tapo, Jan 23, 2013). Computing my own joint accelerations.')

    ##
    # @param arm - 'left_arm' or 'right_arm'
    # @return list of 7 joint velocities in RADIANS/s.
    #         according to meka's coordinate frames.
    def get_joint_velocities(self, arm):
        #return self.arms[arm].get_thetadot_rad().tolist()
        raise RuntimeError('The meka joint velocities seem incorrect. (Advait, July 6, 2011). Computing my own joint velocities.')

    def get_joint_torques(self, arm):
        ''' returns list of 7 joint torques in mNm
        '''
        return self.arms[arm].get_torque_mNm().tolist()

    def get_motor_temps(self, arm):
        ''' returns list of 7 motor temperatures in Celsius
        '''
        return self.arms[arm].get_motor_temp_C().tolist()

    def get_joint_angles(self, arm):
        ''' returns list of 7 joint angles in RADIANS.
            according to meka's coordinate frames.
        '''
        return self.arms[arm].get_theta_rad().tolist()

    def l_jep_cb(self, msg):
        self.cb_lock.acquire()
        self.l_jep = msg.data
        self.cb_lock.release()

    def r_jep_cb(self, msg):
        self.cb_lock.acquire()
        self.r_jep = msg.data
        self.cb_lock.release()

    ##
    # @param q - list of 7 joint angles in RADIANS. according to meka's coordinate frames.
    def set_jep(self, arm, q):
        if self.arm_settings[arm] == None:
            return
        if self.arm_settings[arm].control_mode != 'theta_gc' and \
            self.arm_settings[arm].control_mode != 'wrist_theta_gc':
            raise RuntimeError('Bad control mode: %s'%(self.arm_settings[arm].control_mode))

        for i,qi in enumerate(q):
            ## NOTE - meka's set_theta_deg takes angle in radians.
            #self.joint_list_dict[arm][i].set_theta_deg(qi)
            # Not anymore. (Advait Aug 27, 2009)
            self.joint_list_dict[arm][i].set_theta_rad(qi)

        self.cb_lock.acquire()
        if arm == 'right_arm':
            self.r_jep = q
        else:
            self.l_jep = q
        self.cb_lock.release()

    def r_alpha_cb(self, msg):
        self.cb_lock.acquire()
        self.arm_settings['right_arm'].set_stiffness_scale(list(msg.data))
        self.cb_lock.release()

    def l_alpha_cb(self, msg):
        self.cb_lock.acquire()
        self.arm_settings['left_arm'].set_stiffness_scale(list(msg.data))
        self.cb_lock.release()

    def set_alpha(self, arm, alpha):
        jcl = self.joint_list_dict[arm]
        for i,a in enumerate(alpha):
            jcl[i].set_stiffness(a)



if __name__ == '__main__':
    import optparse

    p = optparse.OptionParser()
    p.add_option('--enable_net_ft_r', action='store_true', dest='netft_r',
                 help='use net ft for the right arm')
    p.add_option('--enable_net_ft_l', action='store_true', dest='netft_l',
                 help='use net ft for the left arm') #Tiffany 11/11/2011
    p.add_option('--arm_to_use', action='store', dest='arm_to_use',
                 help='use only one arm (l or r)', type='string')
    opt, args = p.parse_args()

    try:
        settings_r = MekaArmSettings(stiffness_list=[0.1939,0.6713,0.748,0.7272,0.75])
        #settings_r = None
        settings_l = MekaArmSettings(stiffness_list=[0.1939,0.6713,0.748,0.7272,0.75])
        #settings_l = None

        if opt.arm_to_use == 'l':
            enable_left_arm = True
            enable_right_arm = False
        elif opt.arm_to_use == 'r':
            enable_left_arm = False
            enable_right_arm = True
        elif opt.arm_to_use == 'b':
            enable_right_arm = True
            enable_left_arm = True
        else:
            raise RuntimeError('Unrecognized value for the arm_to_use command line argument')

        cody_arms = MekaArmServer(settings_r, settings_l, opt.netft_r,
                                  opt.netft_l, enable_right_arm,
                                  enable_left_arm)

#        print 'hit a key to power up the arms.'
#        k=m3t.get_keystroke()
        cody_arms.power_on()

        while not rospy.is_shutdown():
            cody_arms.step_ros()
            rospy.sleep(0.005)
        cody_arms.stop()

    except m3t.M3Exception:
        print '############################################################'
        print 'In all likelihood the Meka server is not running.'
        print '############################################################'
        raise
    except:
        # only use cody_arms if it was successfully created.
        if 'cody_arms' in locals():
            cody_arms.stop()
        raise