Ask Your Question
0

position and velocity mode control simultaniously for Baxter robot

asked 2021-06-26 15:35:24 -0600

Confused Girl gravatar image

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 ...
(more)
edit retag flag offensive close merge delete

Comments

Publishing at 500 Hz is an very high rate for python. I believe the original baxter code is 100 Hz, and I consider even that to be pushing the limit. To debug this problem, I would use a slower cmd publishing rate (maybe 10Hz?) and, if the joint allows it, ask it to rotate at constant speed in one direction.

Mike Scheutzow gravatar image Mike Scheutzow  ( 2021-06-27 10:15:30 -0600 )edit

1 Answer

Sort by » oldest newest most voted
0

answered 2021-06-27 10:02:09 -0600

Mike Scheutzow gravatar image

As far as I know, the baxter API does not support having some joints in position mode, and other joints in velocity mode, so what you want to do is not possible.

However, for velocity mode, sending a cmd to set some joints to 0 radians/sec should work like anyone would expect. The behavior that "the arm falls down" says to me that your code may be triggering an emergency stop.

My advice is to go back to the original wobbler code, and make one small change at a time to determine where the problem is in your code.

edit flag offensive delete link more

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

2 followers

Stats

Asked: 2021-06-26 15:35:24 -0600

Seen: 76 times

Last updated: Jun 27 '21