def main():
print('sup')
rospy.init_node('sawyer_kinematics')
print('init')
# rospy.sleep(10)
# init node????
# step 1 determine initial point
# step 2 determine desired endpoint
# in loop now
# step 3 measure force and position
# step 4 determine force we need to apply to push towards desired endpoint
# step 5 convert force to joint torques with Jacobian
# step 6 command joint torques
# end of loop
r = rospy.Rate(200)
limb = Limb()
kin = sawyer_kinematics('right')
#step 1
pose = limb.endpoint_pose()
point = pose.get('position')
quaternion = pose.get('orientation') # both are 3x
#step 2
path = np.array([.1, 0, 0])
des_point = point + path
des_quaternion = quaternion
path_dir = normalize(path)
#step 3
#stiffness
alpha = 1 #multiplier
while not rospy.is_shutdown() and error > tol:
#step 3
force = get_end_force(limb)
# print("force is:", force)
f_mag = force_mag(force)
f_dir = force_dir(force)
#step 4
f_right = proj(path, force) * path_dir
f_wrong = force - f_right
force_apply = f_right * alpha - f_wrong * K
force_apply = np.hstack((force_apply, np.array([0, 0, 0])))
# print('force_apply is:', force_apply)
#step 5
Jt = kin.jacobian_transpose()
joint_torques = np.dot(Jt, force_apply) #make sure right dims
joint_torques = np.asarray(joint_torques)
joint_torques = np.array([[0, 0, 0, 0, 0, 0, 0]])
# print('set joints as:', joint_torques[0])
set_torques = joint_array_to_dict(joint_torques[0], limb)
print(set_torques)
print()
#step 6
# limb.set_joint_torques(set_torques)
cur_point = limb.endpoint_pose().get('position')
error = np.linalg.norm(des_point - cur_point)
r.sleep()
def move_position(xs, ys, zs):
limb = intera_interface.Limb('right')
kinematics = sawyer_kinematics('right')
xyz_pos = [xs, ys, zs]
x = dict(zip(limb._joint_names, kinematics.inverse_kinematics(xyz_pos)))
limb.move_to_joint_positions(x)
rospy.sleep(1.0)
def attach_springs(self):
"""
Switches to joint torque mode and attached joint springs to current
joint positions.
"""
# record initial joint angles
self._start_angles = self._limb.joint_angles()
# set control rate
control_rate = rospy.Rate(self._rate)
# for safety purposes, set the control rate command timeout.
# if the specified number of command cycles are missed, the robot
# will timeout and return to Position Control Mode
self._limb.set_command_timeout((1.0 / self._rate) * self._missed_cmds)
limb = Limb()
kin = sawyer_kinematics('right')
pose = limb.endpoint_pose()
point = pose.get('position')
quaternion = pose.get('orientation')
# loop at specified rate commanding new joint torques
while not rospy.is_shutdown():
if not self._rs.state().enabled:
rospy.logerr("Joint torque example failed to meet "
"specified control rate timeout.")
break
self._update_forces(limb, kin, point, quaternion)
control_rate.sleep()
def __init__(self, reconfig_server, limb = "right"):
self._dyn = reconfig_server
# control parameters
self._rate = 1000.0 # Hz
self._missed_cmds = 20.0 # Missed cycles before triggering timeout
# create our limb instance
self._limb = intera_interface.Limb(limb)
# initialize parameters
self._springs = dict()
self._damping = dict()
self._start_angles = dict()
kinematics = sawyer_kinematics("right")
xyz_pos = [4, 0, 0]
x = dict(zip(self._limb._joint_names, kinematics.inverse_kinematics(xyz_pos)))
self._limb.set_joint_positions(x)
# create cuff disable publisher
cuff_ns = 'robot/limb/' + limb + '/suppress_cuff_interaction'
self._pub_cuff_disable = rospy.Publisher(cuff_ns, Empty, queue_size=1)
# verify robot is enabled
print("Getting robot state... ")
self._rs = intera_interface.RobotEnable(CHECK_VERSION)
self._init_state = self._rs.state().enabled
print("Enabling robot... ")
self._rs.enable()
print("Running. Ctrl-c to quit")
def __init__(self):
global LEGO_HEIGHT
self.lego_height = LEGO_HEIGHT
self.currentPos = [None, None, None] #length must be 3, [x,y,z]
self.gripper = intera_interface.Gripper()
self.limb = intera_interface.Limb('right')
self.kinematics = sawyer_kinematics('right')
self.currentLegoPos = [None, None, None]
def __init__(self):
self.arm = "right"
self.limb = intera_interface.Limb(
self.arm) # Create arm & limb objects
self.kin = sawyer_kinematics(self.arm) # Create kinematics construct
self.rs = intera_interface.RobotEnable() # Enable Saywer
self.rs.enable()
def __init__(self):
# Instance Variables
self.P_gain = 2e-3
self.I_gain = 0
self.D_gain = 0
self.int_err_x = 0
self.int_err_y = 0
self.x_bnd = [-0.27, 0.38]
self.y_bnd = [0.35, 0.85]
self.last_err_x = 0
self.last_err_y = 0
# Initialize Sawyer
rospy.init_node('sawyer_comm', anonymous=True)
self.limb = intera_interface.Limb('right')
self.kin = sawyer_kinematics('right')
joint_positions = go_to.joint_angle_arg(joint_angles=[
math.pi / 2, -math.pi / 3, 0, 2 * math.pi / 3, 0, -math.pi / 3,
0.57
],
speed_ratio=.225,
accel_ratio=0.1)
go_to.joint_angles(joint_positions)
self.start_pose = self.limb.endpoint_pose()
if VERBOSE:
self.lastTime = rospy.get_time()
'''
# Debug: move in a square
s = 0.1
self.limb.set_command_timeout(2)
rate = rospy.Rate(100)
x = np.array([s,0,0,0,0,0]);
while self.limb.endpoint_pose()['position'][0]<0.25:
self.set_endpoint_velocity(x)
rate.sleep()
x = np.array([0,-s,0,0,0,0]);
while self.limb.endpoint_pose()['position'][1]>0.46:
self.set_endpoint_velocity(x)
rate.sleep()
x = np.array([-s,0,0,0,0,0]);
while self.limb.endpoint_pose()['position'][0]>-0.1658:
self.set_endpoint_velocity(x)
rate.sleep()
x = np.array([0,s,0,0,0,0]);
while self.limb.endpoint_pose()['position'][1]<0.7206:
self.set_endpoint_velocity(x)
rate.sleep()
x = np.array([0,0,0,0,0,0]);
self.set_endpoint_velocity(x)
'''
# Subscribing topics
rospy.Subscriber('/ball_error',
Int32MultiArray,
callback=self.rx_pos_error,
queue_size=1)
def main():
rospy.init_node('sawyer_kinematics')
print '*** Sawyer PyKDL Kinematics ***\n'
kin = sawyer_kinematics('right')
print '\n*** Sawyer Description ***\n'
kin.print_robot_description()
print '\n*** Sawyer KDL Chain ***\n'
kin.print_kdl_chain()
# FK Position
print '\n*** Saywer Position FK ***\n'
print kin.forward_position_kinematics()
# FK Velocity
# print '\n*** Sawyer Velocity FK ***\n'
# kin.forward_velocity_kinematics()
# IK
print '\n*** Sawyer Position IK ***\n'
pos = [0.582583, -0.180819, 0.216003]
rot = [0.03085, 0.9945, 0.0561, 0.0829]
print kin.inverse_kinematics(pos) # position, don't care orientation
print '\n*** Sawyer Pose IK ***\n'
print kin.inverse_kinematics(pos, rot) # position & orientation
# Jacobian
print '\n*** Sawyer Jacobian ***\n'
print kin.jacobian()
# Jacobian Transpose
print '\n*** Sawyer Jacobian Tranpose***\n'
print kin.jacobian_transpose()
# Jacobian Pseudo-Inverse (Moore-Penrose)
print '\n*** Sawyer Jacobian Pseudo-Inverse (Moore-Penrose)***\n'
print kin.jacobian_pseudo_inverse()
# Joint space mass matrix
print '\n*** Sawyer Joint Inertia ***\n'
print kin.inertia()
# Cartesian space mass matrix
print '\n*** Sawyer Cartesian Inertia ***\n'
print kin.cart_inertia()
# Joint space coriolis matrix
print '\n*** Sawyer Joint Coriolis ***\n'
print kin.coriolis()
# Cartesian space coriolis matrix
# print '\n*** Sawyer Cartesian Coriolis ***\n'
# print kin.cart_coriolis()
# Joint space gravity matrix
print '\n*** Sawyer Joint Gravity ***\n'
print kin.gravity()
def __init__(self):
self.arm = "right"
self.limb = intera_interface.Limb(self.arm)
self.joint_names = self.limb.joint_names()
self.kin = sawyer_kinematics(self.arm)
self.limb.set_joint_position_speed(0.25)
self.rs = intera_interface.RobotEnable()
self.rs.enable()
self._state = "LEFT2RIGHT"
neutral = [
-1.186115234375, 0.805931640625, 2.448353515625, 1.99850390625,
-2.3551396484375, -0.67713671875, 3.3021533203125, 0.0
]
self.neutral = dict(zip(self.joint_names, neutral))
def main():
rospy.init_node('testing_node')
limb = Limb('right')
print('yay')
r = rospy.Rate(200)
kin = sawyer_kinematics('right')
Jt = kin.jacobian_transpose()
F = np.array([0, 0, 0, 0, 0, 10])
tau = np.dot(Jt, F)
# print(tau)
# rospy.sleep(20)
# limb.set_command_timeout(.1)
# wrench = limb.endpoint_effort()
# force = wrench['force']
# print("force type: ", force)
# print(wrench)
while not rospy.is_shutdown():
# joint_torques = np.array([[0,0,0,0,0,0,0]])
Jt = kin.jacobian_transpose()
mcart = kin.cart_inertia()
f = np.array([0, 0, -9.8, 0, 0, 0])
gravity = np.dot(np.dot(Jt, mcart), f)
joint_torques = np.array([
1.5474950095623006, -16.78679653980678, -2.8277487768926406,
-0.1771867794616826, 0.1073210015442511, -0.5216893350253217,
-0.00607477942479895
])
# cur = limb.joint_efforts()
# print('set joints as:', joint_torques[0])
# set_torques = joint_array_to_dict(joint_torques[0], limb)
# print(set_torques)
# print(set_torques.get('right_j1'))
# torque = np.array([gravity.item(i) for i in range(7)])
torque = joint_array_to_dict(joint_torques, limb)
# cur['right_j1'] = 0
# cur['right_j2'] = 0
print('joint torques', torque)
limb.set_joint_torques(torque)
r.sleep()
def __init__(self,
limb="right",
tip_name="right_gripper_tip",
hover_distance=0.1):
self._limb_name = limb
self._tip_name = tip_name
self._limb = intera_interface.Limb(limb)
# verify robot is enabled
print("Getting robot state... ")
self._rs = intera_interface.RobotEnable(intera_interface.CHECK_VERSION)
self._init_state = self._rs.state().enabled
print("Enabling robot... ")
self._rs.enable()
self._joint_names = [
'right_j0', 'right_j1', 'right_j2', 'right_j3', 'right_j4',
'right_j5', 'right_j6'
]
self._kin = sawyer_kinematics(limb)
self.prev_pos = np.zeros(3)
self.prev_time = None
# Sample program to solve for the Inverse Kinematics of a particular hand position and send the robot arm there
import rospy
import intera_interface
from sawyer_pykdl import sawyer_kinematics
rospy.init_node("test_ik")
limb = intera_interface.Limb('right')
kinematics = sawyer_kinematics('right')
xyz_pos = [0.58, -0.48, 0.216]
x = dict(zip(limb._joint_names, kinematics.inverse_kinematics(xyz_pos)))
limb.move_to_joint_positions(x)
def __init__(self):
print "Initializing Node"
rospy.init_node('poirot_rrhost')
print "Enabling Robot"
rs = intera_interface.RobotEnable()
rs.enable()
self.rs1 = intera_interface.RobotEnable()
rospy.Subscriber("spacenav/joy", Joy, self.SpaceNavJoyCallback)
rospy.Subscriber("/robot/digital_io/right_lower_cuff/state",
DigitalIOState, self.CuffCallback)
rospy.Subscriber("/robot/digital_io/right_button_ok/state",
DigitalIOState, self.NavWheelCallback)
rospy.Subscriber("/robot/digital_io/right_button_triangle/state",
DigitalIOState, self.XCallback)
self.DIOpub = rospy.Publisher('/io/comms/io/command',
IOComponentCommand,
queue_size=10)
self.h = IOComponentCommand()
self.h.time = rospy.Time.now()
self.h.op = "set"
self.h.args = "{ \"signals\" : { \"port_sink_0\" : { \"format\" : { \"type\" : \"int\", \"dimensions\" : [ 1] }, \"data\" : [ 0] } } }"
self.nav = intera_interface.Navigator()
#self.nav.register_callback(self.ok_pressed, '_'.join([nav_name, 'button_ok']))
#intera_interface/robot/digital_io/right_lower_cuff/state
# self._valid_limb_names = {'left': 'left',
# 'l': 'left',
# 'right': 'right',
# 'r': 'right'}
self._valid_limb_names = {'right': 'right', 'r': 'right'}
# get information from the SDK
# self._left = intera_interface.Limb('left')
self._right = intera_interface.Limb('right')
#self._l_jnames = self._left.joint_names()
self._r_jnames = self._right.joint_names()
self.kin = sawyer_kinematics('right')
# data initializations
self._jointpos = [0] * 7
self._jointvel = [0] * 7
self._jointtor = [0] * 7
self._ee_pos = [0] * 3
self._ee_or = [0] * 4
self._ee_tw = [0] * 6
self._ee_wr = [0] * 6
self._ee_vel = [0] * 6
self._pijac = [
] #numpy.matrix('0,0,0,0,0,0;0,0,0,0,0,0;0,0,0,0,0,0;0,0,0,0,0,0;0,0,0,0,0,0;0,0,0,0,0,0;0,0,0,0,0,0')
self._jac = [
] #numpy.matrix('0,0,0,0,0,0;0,0,0,0,0,0;0,0,0,0,0,0;0,0,0,0,0,0;0,0,0,0,0,0;0,0,0,0,0,0;0,0,0,0,0,0')
self._inertia_mat = [
] #numpy.matrix('0,0,0,0,0,0,0;0,0,0,0,0,0,0;0,0,0,0,0,0,0;0,0,0,0,0,0,0;0,0,0,0,0,0,0;0,0,0,0,0,0,0;0,0,0,0,0,0,0')
self.Jxl_raw = 0
self.Jyl_raw = 0
self.Jzl_raw = 0
self.Jxa_raw = 0
self.Jya_raw = 0
self.Jza_raw = 0
self.leftbutton = 0
self.rightbutton = 0
self.spacenav = []
#self._l_joint_command = dict(zip(self._l_jnames,[0.0]*7))
self._r_joint_command = dict(zip(self._r_jnames, [0.0] * 7))
self.MODE_POSITION = 0
self.MODE_VELOCITY = 1
self.MODE_TORQUE = 2
self._mode = self.MODE_POSITION
self.RMH_delay = .01
# initial joint command is current pose
self.readJointPositions()
#self.setJointCommand('left',self._jointpos[0:7])
self.setJointCommand('right', self._jointpos[0:7])
self.head_display = intera_interface.HeadDisplay()
print('head display!')
self.image = "/home/rachel/ros_ws/src/sawyer_simulator/sawyer_sim_examples/models/cafe_table/materials/textures/Wood_Floor_Dark.jpg"
self.head_display.display_image(self.image, display_rate=100)
print('finished first head display')
# Start background threads
self._running = True
self._t_joints = threading.Thread(target=self.jointspace_worker)
self._t_joints.daemon = True
self._t_joints.start()
self._t_effector = threading.Thread(target=self.endeffector_worker)
self._t_effector.daemon = True
self._t_effector.start()
self._t_command = threading.Thread(target=self.command_worker)
self._t_command.daemon = True
self._t_command.start()
self._t_display = threading.Thread(target=self.display_worker)
self._t_display.daemon = True
self._t_display.start()
self._t_dio = threading.Thread(target=self.dio_worker)
self._t_dio.daemon = True
self._t_dio.start()
import rospy
import intera_interface
from sawyer_pykdl import sawyer_kinematics
rospy.init_node("ik")
limb = intera_interface.Limb("right")
kinematics = sawyer_kinematics("right")
import argparse
parser = argparse.ArgumentParser(description="A tutorial of argparse!")
parser.add_argument("pos", default=[0, 0, 0, 0, 0, 0])
args = parser.parse_args()
a = args.a
joints = kinematics.inverse_kinematics(pos)
if (joints != None):
dest = zip(limb._joint_names, joints)
limb.move_to_joint_positions(dict(dest))
else:
print("not a valid location")
def __init__(self):
print "Initializing Node"
rospy.init_node('poirot_rrhost')
print "Enabling Robot"
rs = intera_interface.RobotEnable()
rs.enable()
self.rs1 = intera_interface.RobotEnable()
rospy.Subscriber("spacenav/joy", Joy, self.SpaceNavJoyCallback)
rospy.Subscriber("/robot/digital_io/right_lower_cuff/state",
DigitalIOState, self.CuffCallback)
rospy.Subscriber("/robot/digital_io/right_button_ok/state",
DigitalIOState, self.NavWheelCallback)
rospy.Subscriber("/robot/digital_io/right_button_triangle/state",
DigitalIOState, self.XCallback)
rospy.Subscriber("/robot/digital_io/right_lower_button/state",
DigitalIOState, self.GripperCloseCallback)
rospy.Subscriber("/robot/digital_io/right_upper_button/state",
DigitalIOState, self.GripperOpenCallback)
rospy.Subscriber("/pushed1", Bool, self.pushed1callback)
rospy.Subscriber("/pushed2", Bool, self.pushed2callback)
rospy.Subscriber("/pushed3", Bool, self.pushed3callback)
rospy.Subscriber("/pushed4", Bool, self.pushed4callback)
self.DIOpub = rospy.Publisher('/io/comms/io/command',
IOComponentCommand,
queue_size=10)
self.h = IOComponentCommand()
self.h.time = rospy.Time.now()
self.h.op = "set"
self.h.args = "{ \"signals\" : { \"port_sink_0\" : { \"format\" : { \"type\" : \"int\", \"dimensions\" : [ 1] }, \"data\" : [ 0] } } }"
rospy.Subscriber("Robotiq2FGripperRobotInput",
inputMsg.Robotiq2FGripper_robot_input,
self.GripperStatusCallback)
self.gripperpub = rospy.Publisher(
'Robotiq2FGripperRobotOutput',
outputMsg.Robotiq2FGripper_robot_output)
self.grippercommand = outputMsg.Robotiq2FGripper_robot_output()
self.gripperstatus = inputMsg.Robotiq2FGripper_robot_input()
self.grip_des_pos = 444
#self.des_gripper_pos=0
#activate-reset-activate gripper
self.grippercommand.rACT = 1
self.grippercommand.rGTO = 1
self.grippercommand.rSP = 255
self.grippercommand.rFR = 150
self.gripperpub.publish(self.grippercommand)
time.sleep(.5)
self.grippercommand = outputMsg.Robotiq2FGripper_robot_output()
self.grippercommand.rACT = 0
self.gripperpub.publish(self.grippercommand)
time.sleep(.5)
self.grippercommand = outputMsg.Robotiq2FGripper_robot_output()
self.grippercommand.rACT = 1
self.grippercommand.rGTO = 1
self.grippercommand.rSP = 255
self.grippercommand.rFR = 150
self.pb1 = 1
self.pb2 = 1
self.pb3 = 1
self.pb4 = 1
time.sleep(.5)
self.nav = intera_interface.Navigator()
self.head = intera_interface.Head()
self.headangle = 0
self._valid_limb_names = {'right': 'right', 'r': 'right'}
# get information from the SDK
# self._left = intera_interface.Limb('left')
self._right = intera_interface.Limb('right')
#self._l_jnames = self._left.joint_names()
self._r_jnames = self._right.joint_names()
self.kin = sawyer_kinematics('right')
# data initializations
self._jointpos = [0] * 7
self._jointvel = [0] * 7
self._jointtor = [0] * 7
self._ee_pos = [0] * 3
self._ee_or = [0] * 4
self._ee_tw = [0] * 6
self._ee_wr = [0] * 6
self._ee_vel = [0] * 6
self._pijac = [
] #numpy.matrix('0,0,0,0,0,0;0,0,0,0,0,0;0,0,0,0,0,0;0,0,0,0,0,0;0,0,0,0,0,0;0,0,0,0,0,0;0,0,0,0,0,0')
self._jac = [
] #numpy.matrix('0,0,0,0,0,0;0,0,0,0,0,0;0,0,0,0,0,0;0,0,0,0,0,0;0,0,0,0,0,0;0,0,0,0,0,0;0,0,0,0,0,0')
self._inertia_mat = [
] #numpy.matrix('0,0,0,0,0,0,0;0,0,0,0,0,0,0;0,0,0,0,0,0,0;0,0,0,0,0,0,0;0,0,0,0,0,0,0;0,0,0,0,0,0,0;0,0,0,0,0,0,0')
self.Jxl_raw = 0
self.Jyl_raw = 0
self.Jzl_raw = 0
self.Jxa_raw = 0
self.Jya_raw = 0
self.Jza_raw = 0
self.leftbutton = 0
self.rightbutton = 0
self.spacenav = []
#self._l_joint_command = dict(zip(self._l_jnames,[0.0]*7))
self._r_joint_command = dict(zip(self._r_jnames, [0.0] * 7))
self.MODE_POSITION = 0
self.MODE_VELOCITY = 1
self.MODE_TORQUE = 2
self._mode = self.MODE_POSITION
self.RMH_delay = .01
# initial joint command is current pose
self.readJointPositions()
#self.setJointCommand('left',self._jointpos[0:7])
self.setJointCommand('right', self._jointpos[0:7])
self.hascollided = 0
self.head_display = intera_interface.HeadDisplay()
print('head display!')
#self.image="/home/rachel/rawhide/rawhide_ws/src/sawyer_simulator/sawyer_sim_examples/models/cafe_table/materials/textures/Wood_Floor_Dark.jpg"
self.image = "/home/rachel/rawhide/rmh_code/puppy.png"
self.head_display.display_image(self.image, display_rate=100)
print('finished first head display')
# Start background threads
self._running = True
self._t_joints = threading.Thread(target=self.jointspace_worker)
self._t_joints.daemon = True
self._t_joints.start()
self._t_effector = threading.Thread(target=self.endeffector_worker)
self._t_effector.daemon = True
self._t_effector.start()
self._t_command = threading.Thread(target=self.command_worker)
self._t_command.daemon = True
self._t_command.start()
self._t_display = threading.Thread(target=self.display_worker)
self._t_display.daemon = True
self._t_display.start()
self._t_dio = threading.Thread(target=self.dio_worker)
self._t_dio.daemon = True
self._t_dio.start()
def __init__(self):
print "Initializing Node"
rospy.init_node('sawyer_jointstates')
print "Enabling Robot"
rs = intera_interface.RobotEnable()
rs.enable()
# self._valid_limb_names = {'left': 'left',
# 'l': 'left',
# 'right': 'right',
# 'r': 'right'}
self._valid_limb_names = {'right': 'right', 'r': 'right'}
# get information from the SDK
# self._left = intera_interface.Limb('left')
self._right = intera_interface.Limb('right')
#self._l_jnames = self._left.joint_names()
self._r_jnames = self._right.joint_names()
self.kin = sawyer_kinematics('right')
# data initializations
self._jointpos = [0] * 7
self._jointvel = [0] * 7
self._jointtor = [0] * 7
self._ee_pos = [0] * 3
self._ee_or = [0] * 4
self._ee_tw = [0] * 6
self._ee_wr = [0] * 6
self._ee_vel = [0] * 6
self._pijac = [
] #numpy.matrix('0,0,0,0,0,0;0,0,0,0,0,0;0,0,0,0,0,0;0,0,0,0,0,0;0,0,0,0,0,0;0,0,0,0,0,0;0,0,0,0,0,0')
self._jac = [
] #numpy.matrix('0,0,0,0,0,0;0,0,0,0,0,0;0,0,0,0,0,0;0,0,0,0,0,0;0,0,0,0,0,0;0,0,0,0,0,0;0,0,0,0,0,0')
self._inertia_mat = [
] #numpy.matrix('0,0,0,0,0,0,0;0,0,0,0,0,0,0;0,0,0,0,0,0,0;0,0,0,0,0,0,0;0,0,0,0,0,0,0;0,0,0,0,0,0,0;0,0,0,0,0,0,0')
#self._l_joint_command = dict(zip(self._l_jnames,[0.0]*7))
self._r_joint_command = dict(zip(self._r_jnames, [0.0] * 7))
self.MODE_POSITION = 0
self.MODE_VELOCITY = 1
self.MODE_TORQUE = 2
self._mode = self.MODE_POSITION
self.RMH_delay = .01
# initial joint command is current pose
self.readJointPositions()
#self.setJointCommand('left',self._jointpos[0:7])
self.setJointCommand('right', self._jointpos[0:7])
# Start background threads
self._running = True
self._t_joints = threading.Thread(target=self.jointspace_worker)
self._t_joints.daemon = True
self._t_joints.start()
self._t_effector = threading.Thread(target=self.endeffector_worker)
self._t_effector.daemon = True
self._t_effector.start()
self._t_command = threading.Thread(target=self.command_worker)
self._t_command.daemon = True
self._t_command.start()
