position and velocity mode control simultaniously for Baxter robot
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 ...
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.