position and velocity mode control simultaniously for Baxter robot

edit

Hello everyone. I am working with Baxter and this robot is very new to me. I am trying to fix all joints of the right arm of the baxter to an specific position and then control a single joint of the arm velocity. The velocity is in the form of a sinusoidal function. I tried to modify the joint_velocity_wobbler example of Baxter and defined my starting position for all the joints. So the robot first moves joints to the position that I want. Then switches to velocity mode control and starts moving one joint based on the velocity command but the problem is all other joints get loose and they do not stay at the starting position. The arm comes down. How I can fix the problem? how can I switch to velocity mode control for one joint only and keep other joints to be controlled based on position mode control? here is the code. Thank you in advance.

import argparse import math import random import rospy from std_msgs.msg import ( UInt16, )

import baxter_interface from baxter_interface import CHECK_VERSION

class Wobbler(object):

def __init__(self):
    """
    'Wobbles' one arms by commanding joint velocities sinusoidally.
    """
    self._pub_rate = rospy.Publisher('robot/joint_state_publish_rate',
                                     UInt16, queue_size=10)
    #self._left_arm = baxter_interface.limb.Limb("left")
    self._right_arm = baxter_interface.limb.Limb("right")
    #self._left_joint_names = self._left_arm.joint_names()
    self._right_joint_names = self._right_arm.joint_names()

    # control parameters
    self._rate = 500.0  # Hz

    print("Getting robot state... ")
    self._rs = baxter_interface.RobotEnable(CHECK_VERSION)
    self._init_state = self._rs.state().enabled
    print("Enabling robot... ")
    self._rs.enable()

    # set joint state publishing to 500Hz
    self._pub_rate.publish(self._rate)

def _reset_control_modes(self):
    rate = rospy.Rate(self._rate)
    for _ in xrange(100):
        if rospy.is_shutdown():
            return False

        #self._left_arm.exit_control_mode()
        self._right_arm.exit_control_mode()
        self._pub_rate.publish(100)  # 100Hz default joint state rate
        rate.sleep()
    return True

def set_neutral(self):
    """
    Sets right arm back into a starting pose.
    """
    starting_joints_angles={"right_s0": -0.6,
                            "right_s1": 0,
                            "right_e0": -3,
                            "right_e1": -1.2156797743991905,
                            "right_w0": -0.2174417766827574,
                            "right_w1": 1.3426166845967085,
                            "right_w2": 0.029912625363765568}

    print("Moving to starting pose...")
    self._right_arm.move_to_joint_positions(starting_joints_angles)

def clean_shutdown(self):

    print("\nExiting example...")
    #return to normal
    self._reset_control_modes()
    self.set_neutral()
    if not self._init_state:
        print("Disabling robot...")
        self._rs.disable()
    return True

def wobble(self):
    self.set_neutral()

    rate = rospy.Rate(self._rate)
    start = rospy.Time.now()
#print(start)

    def make_v_func():
        """
        returns a randomly parameterized cosine function to control a
        specific joint.
        """
        period_factor = random.uniform(0.3, 0.5)
        amplitude_factor = random.uniform(0.1, 0.2)

        def v_func(elapsed):
            w = period_factor * elapsed.to_sec()
            return amplitude_factor * math.cos(w * 2 * math.pi)
        return v_func

    v_funcs = [make_v_func() for _ in self._right_joint_names]
#v_funcs = [0.001 for _ in self._right_joint_names]

#print(v_funcs[0])

    def make_cmd(joint_names, elapsed):
        A=dict([(joint, v_funcs[i](elapsed))
                     for i, joint in enumerate(joint_names)])
        #print((A['right_s0']))
        A["right_s0"]=0
        A["right_w0"]=0
        A["right_e0"]=0
        A["right_w2"]=0
        A["right_s1"]=0
        A["right_w1"]=10*A["right_w1"]
        A["right_e1"]=0

        print(A)

        return A
    """
    {'right_s0': 0, 'right_s1': -0.06405320376380977, 'right_w0': 0.020231814795889494, 'right_w1': -0.11502538983026409, 'right_w2 ...