def record_good_configs(use_left_arm):
import m3.toolbox as m3t
settings_arm = ar.MekaArmSettings(stiffness_list=[0.,0.,0.,0.,0.],control_mode='torque_gc')
if use_left_arm:
firenze = ar.M3HrlRobot(connect=True,left_arm_settings=settings_arm)
arm = 'left_arm'
else:
firenze = ar.M3HrlRobot(connect=True,right_arm_settings=settings_arm)
arm = 'right_arm'
print 'hit ENTER to start the recording process'
k=m3t.get_keystroke()
firenze.power_on()
good_configs_list = []
while k == '\r':
print 'hit ENTER to record configuration, something else to exit'
k=m3t.get_keystroke()
firenze.proxy.step()
q = firenze.get_joint_angles(arm)
good_configs_list.append(np.matrix(q).A1.tolist())
firenze.stop()
ut.save_pickle(good_configs_list,ut.formatted_time()+'_good_configs_list.pkl')
def pose_arm(self,hook_angle):
print 'press ENTER to pose the robot.'
k=m3t.get_keystroke()
if k!='\r':
print 'You did not press ENTER.'
return
# settings_r_orig = copy.copy(self.firenze.arm_settings['right_arm'])
settings_torque_gc = hr.MekaArmSettings(stiffness_list=[0.,0.,0.,0.,0.],control_mode='torque_gc')
self.firenze.set_arm_settings(settings_torque_gc,None)
self.firenze.step()
print 'hit ENTER to end posing, something else to exit'
k=m3t.get_keystroke()
p = self.firenze.end_effector_pos('right_arm')
q = self.firenze.get_joint_angles('right_arm')
# self.firenze.set_arm_settings(settings_r_orig,None)
self.firenze.set_arm_settings(self.settings_stiff,None)
self.firenze.set_joint_angles('right_arm',q)
self.firenze.step()
self.firenze.set_joint_angles('right_arm',q)
self.firenze.step()
rot_mat = tr.Rz(hook_angle-hook_3dprint_angle)*tr.Ry(math.radians(-90))
self.firenze.go_cartesian('right_arm',p,rot_mat,speed=0.1)
print 'hit ENTER after making finer adjustment, something else to exit'
k=m3t.get_keystroke()
p = self.firenze.end_effector_pos('right_arm')
q = self.firenze.get_joint_angles('right_arm')
self.firenze.set_joint_angles('right_arm',q)
self.firenze.step()
def record_good_configs(use_left_arm):
import m3.toolbox as m3t
settings_arm = ar.MekaArmSettings(stiffness_list=[0., 0., 0., 0., 0.],
control_mode='torque_gc')
if use_left_arm:
firenze = ar.M3HrlRobot(connect=True, left_arm_settings=settings_arm)
arm = 'left_arm'
else:
firenze = ar.M3HrlRobot(connect=True, right_arm_settings=settings_arm)
arm = 'right_arm'
print 'hit ENTER to start the recording process'
k = m3t.get_keystroke()
firenze.power_on()
good_configs_list = []
while k == '\r':
print 'hit ENTER to record configuration, something else to exit'
k = m3t.get_keystroke()
firenze.proxy.step()
q = firenze.get_joint_angles(arm)
good_configs_list.append(np.matrix(q).A1.tolist())
firenze.stop()
ut.save_pickle(good_configs_list,
ut.formatted_time() + '_good_configs_list.pkl')
def record_joint_displacements():
print 'hit ENTER to start the recording process'
k=m3t.get_keystroke()
pos_list = []
force_list = []
while k == '\r':
print 'hit ENTER to record configuration, something else to exit'
k=m3t.get_keystroke()
firenze.proxy.step()
pos_list.append(firenze.end_effector_pos('right_arm'))
force_list.append(firenze.get_wrist_force('right_arm', base_frame=True))
ut.save_pickle({'pos_list':pos_list,'force_list':force_list},'stiffness_'+ut.formatted_time()+'.pkl')
firenze.stop()
def run(self):
vias = {}
for c in self.chains:
vias[c] = []
while not self.done:
print '--------------'
print 'r: record via'
print 's: save via file'
print 'p: print current pose'
print 'q: quit'
print '--------------'
print
k = m3t.get_keystroke()
print 'Dbg', dbg
if k == 'r':
print '-------------'
for c in self.chains:
vias[c].append(m3t.float_list(self.bot.get_theta_deg(c)))
print 'Record of: ', vias[c][-1], 'for', c
if k == 'p':
print '----------------'
for c in self.chains:
print 'Chain:', c, ' : ', self.bot.get_theta_deg(c)
print '----------------'
if k == 's':
bot_name = m3t.get_robot_name()
fn = bot_name + '_' + m3t.time_string()
print 'Enter via file name [', fn, ']'
fn = m3t.get_string(fn)
fn = m3t.get_m3_animation_path() + fn + '.via'
print 'Writing file: ', fn
f = file(fn, 'w')
d = {}
for c in self.chains:
ndof = self.bot.get_num_dof(c)
if c == 'torso':
param = {
'slew': [1.0] * ndof,
'stiffness': [0.8] * ndof,
'velocity': [25.0, 15.0]
}
else:
param = {
'slew': [1.0] * ndof,
'stiffness': [0.4] * ndof,
'velocity': [25.0] * ndof
}
d[c] = {
'postures': vias[c],
'param': param
} #safe defaults
f.write(yaml.safe_dump(d))
vias = {}
for c in self.chains:
vias[c] = []
if k == 'q':
self.done = True
def start(self):
print '---------M3 RECORDER--------'
print 'To be recorded : '
for f,a in zip(self.functions_to_call,self.arguments):
print f+'('+a+')'
if self.enable_zero_gravity:
self.set_to_zero_gravity_mode()
if not self.record_now:
print '-------------------------------'
print 'Press any key to start'
print 'q to quit'
c = m3t.get_keystroke()
if c=='q':
return None
self.recorder.start()
print ""
print "Press any key to STOP "
m3t.get_keystroke()
def start(self):
print '---------M3 RECORDER--------'
print 'To be recorded : '
for f, a in zip(self.functions_to_call, self.arguments):
print f + '(' + a + ')'
if a is not '' and self.enable_zero_gravity:
self.set_to_zero_gravity_mode(chain=a)
if not self.record_now:
print '-------------------------------'
print 'Press any key to start'
print 'q to quit'
c = m3t.get_keystroke()
if c == 'q':
return None
self.recorder.start()
print ""
print "Press any key to STOP "
m3t.get_keystroke()
def test_IK(arm, rot_mat):
''' try out the IK at a number of different cartesian
points in the workspace, with the given rotation
matrix for the end effector.
'''
print 'press ENTER to start.'
k=m3t.get_keystroke()
while k=='\r':
p = firenze.end_effector_pos(arm)
firenze.go_cartesian(arm,p,rot_mat,speed=0.1)
firenze.step()
print 'press ENTER to save joint angles.'
k=m3t.get_keystroke()
if k == '\r':
firenze.step()
q = firenze.get_joint_angles(arm)
ut.save_pickle(q,'arm_config_'+ut.formatted_time()+'.pkl')
print 'press ENTER for next IK test. something else to exit.'
k=m3t.get_keystroke()
def do_test(self, ct):
if ct == 'ds':
while True:
print 'Hit q to quit, any other key to display'
if m3t.get_keystroke() == 'q':
break
ts = time.time()
while (time.time() - ts < 5.0):
self.display_sensors()
self.step(0.1)
return False
def do_test(self,ct): if ct=='ds': while True: print 'Hit q to quit, any other key to display' if m3t.get_keystroke()=='q': break ts=time.time() while(time.time()-ts<5.0): self.display_sensors() self.step(0.1) return False
def record_joint_displacements():
print 'hit ENTER to start the recording process'
k = m3t.get_keystroke()
pos_list = []
force_list = []
while k == '\r':
print 'hit ENTER to record configuration, something else to exit'
k = m3t.get_keystroke()
firenze.proxy.step()
pos_list.append(firenze.end_effector_pos('right_arm'))
force_list.append(firenze.get_wrist_force('right_arm',
base_frame=True))
ut.save_pickle({
'pos_list': pos_list,
'force_list': force_list
}, 'stiffness_' + ut.formatted_time() + '.pkl')
firenze.stop()
def record_good_configs(use_left_arm):
import m3.toolbox as m3t
import cody_arm_client as cac
if use_left_arm:
arm = 'l'
else:
arm = 'r'
firenze = cac.CodyArmClient(arm)
print 'hit ENTER to start the recording process'
k=m3t.get_keystroke()
good_configs_list = []
while k == '\r':
print 'hit ENTER to record configuration, something else to exit'
k=m3t.get_keystroke()
q = firenze.get_joint_angles(arm)
good_configs_list.append(np.matrix(q).A1.tolist())
ut.save_pickle(good_configs_list,ut.formatted_time()+'_good_configs_list.pkl')
def pose_arm(self, hook_angle):
print 'press ENTER to pose the robot.'
k = m3t.get_keystroke()
if k != '\r':
print 'You did not press ENTER.'
return
# settings_r_orig = copy.copy(self.firenze.arm_settings['right_arm'])
settings_torque_gc = hr.MekaArmSettings(
stiffness_list=[0., 0., 0., 0., 0.], control_mode='torque_gc')
self.firenze.set_arm_settings(settings_torque_gc, None)
self.firenze.step()
print 'hit ENTER to end posing, something else to exit'
k = m3t.get_keystroke()
p = self.firenze.end_effector_pos('right_arm')
q = self.firenze.get_joint_angles('right_arm')
# self.firenze.set_arm_settings(settings_r_orig,None)
self.firenze.set_arm_settings(self.settings_stiff, None)
self.firenze.set_joint_angles('right_arm', q)
self.firenze.step()
self.firenze.set_joint_angles('right_arm', q)
self.firenze.step()
rot_mat = tr.Rz(hook_angle - hook_3dprint_angle) * tr.Ry(
math.radians(-90))
self.firenze.go_cartesian('right_arm', p, rot_mat, speed=0.1)
print 'hit ENTER after making finer adjustment, something else to exit'
k = m3t.get_keystroke()
p = self.firenze.end_effector_pos('right_arm')
q = self.firenze.get_joint_angles('right_arm')
self.firenze.set_joint_angles('right_arm', q)
self.firenze.step()
def run(self):
vias = {}
for c in self.chains:
vias[c] = []
while not self.done:
print "--------------"
print "r: record via"
print "s: save via file"
print "p: print current pose"
print "q: quit"
print "--------------"
print
k = m3t.get_keystroke()
print "Dbg", dbg
if k == "r":
print "-------------"
for c in self.chains:
vias[c].append(m3t.float_list(self.bot.get_theta_deg(c)))
print "Record of: ", vias[c][-1], "for", c
if k == "p":
print "----------------"
for c in self.chains:
print "Chain:", c, " : ", self.bot.get_theta_deg(c)
print "----------------"
if k == "s":
bot_name = m3t.get_robot_name()
fn = bot_name + "_" + m3t.time_string()
print "Enter via file name [", fn, "]"
fn = m3t.get_string(fn)
fn = m3t.get_m3_animation_path() + fn + ".via"
print "Writing file: ", fn
f = file(fn, "w")
d = {}
for c in self.chains:
ndof = self.bot.get_num_dof(c)
if c == "torso":
param = {"slew": [1.0] * ndof, "stiffness": [0.8] * ndof, "velocity": [25.0, 15.0]}
else:
param = {"slew": [1.0] * ndof, "stiffness": [0.4] * ndof, "velocity": [25.0] * ndof}
d[c] = {"postures": vias[c], "param": param} # safe defaults
f.write(yaml.safe_dump(d))
vias = {}
for c in self.chains:
vias[c] = []
if k == "q":
self.done = True
def do_task(self,ct): #ct: command type #if ct=='gf': #for k in self.calib_default.keys(): #self.reset_sensor(k) #self.comp_rt.load_attr_from_config(self.param_default,self.comp_rt.param) #self.write_config() #return True if ct=='ds': while True: print 'Hit q to quit, any other key to display' if m3t.get_keystroke()=='q': break ts=time.time() while(time.time()-ts<5.0): self.display_sensors() self.step(0.1) return False
def run_ik(proxy, bot, step_delta, arm_name, pub, viz):
pose = get_pose(proxy, bot, arm_name, viz)
bot.set_mode_theta_gc(arm_name)
bot.set_stiffness(arm_name, [stiffness] * bot.get_num_dof(arm_name))
bot.set_theta_deg(arm_name, pose)
#bot.set_theta_deg([0,0,0],[4,5,6]) #No roll/pitch/yaw of wrist
proxy.step()
t = ik_thread(proxy, bot, step_delta, arm_name, pub, viz)
t.start()
d = [0, 0, 0]
while 1:
print '-----------------------'
print 'Delta: ', t.delta
print '-----------------------'
print 'q: quit'
print '1: x+'
print '2: x-'
print '3: y+'
print '4: y-'
print '5: z+'
print '6: z-'
print 'space: step'
k = m3t.get_keystroke()
if k == 'q':
t.delta_done = True
return
if k == '1':
d[0] = d[0] + step_delta
if k == '2':
d[0] = d[0] - step_delta
if k == '3':
d[1] = d[1] + step_delta
if k == '4':
d[1] = d[1] - step_delta
if k == '5':
d[2] = d[2] + step_delta
if k == '6':
d[2] = d[2] - step_delta
t.set_delta(d)
print
print 'Error: ', t.aerror
print 'Target: ', t.target_pos
def run_ik(proxy,bot, step_delta, arm_name,pub, viz):
pose=get_pose(proxy,bot, arm_name,viz)
bot.set_mode_theta_gc(arm_name)
bot.set_stiffness(arm_name, [stiffness]*bot.get_num_dof(arm_name))
bot.set_theta_deg(arm_name, pose)
#bot.set_theta_deg([0,0,0],[4,5,6]) #No roll/pitch/yaw of wrist
proxy.step()
t=ik_thread(proxy,bot,step_delta, arm_name,pub, viz)
t.start()
d=[0,0,0]
while 1:
print '-----------------------'
print 'Delta: ',t.delta
print '-----------------------'
print 'q: quit'
print '1: x+'
print '2: x-'
print '3: y+'
print '4: y-'
print '5: z+'
print '6: z-'
print 'space: step'
k=m3t.get_keystroke()
if k=='q':
t.delta_done=True
return
if k=='1':
d[0]=d[0]+step_delta
if k=='2':
d[0]=d[0]-step_delta
if k=='3':
d[1]=d[1]+step_delta
if k=='4':
d[1]=d[1]-step_delta
if k=='5':
d[2]=d[2]+step_delta
if k=='6':
d[2]=d[2]-step_delta
t.set_delta(d)
print
print 'Error: ',t.aerror
print 'Target: ',t.target_pos
print 'Error: ',t.aerror
print 'Target: ',t.target_pos
# ######################################################
proxy = m3p.M3RtProxy()
proxy.start()
proxy.make_operational_all()
print '--------------------------'
print 'Note: RVIZ support is only intended for debugging in the demo.'
print ' Motor power should be turned off if using RVIZ.'
print 'Use RVIZ? (y/n)'
print '--------------------------'
print
k=m3t.get_keystroke()
rviz = False
pub = None
if k == 'y':
rviz = True
bot_name=m3t.get_robot_name()
if bot_name == "":
print 'Error: no botanoid components found:', bot_names
proxy.stop()
sys.exit()
bot=m3f.create_component(bot_name)
viz = None
if rviz == True:
def pull(self,
hook_angle,
force_threshold,
use_utm=False,
use_camera=False,
strategy='line_neg_x',
pull_loc=None,
info_string=''):
''' force_threshold - max force at which to stop pulling.
hook_angle - radians(0, -90, 90 etc.)
0 - horizontal, -pi/2 hook points up, +pi/2 hook points down
use_utm - to take 3D scans or not.
use_camera - to take pictures from the camera or not.
strategy - 'line_neg_x': move eq point along -x axis.
'piecewise_linear': try and estimate circle and move along it.
'control_radial_force': try and keep the radial force constant
'control_radial_dist'
pull_loc - 3x1 np matrix of location for pulling. If None then arm will go into
gravity comp and user can show the location.
info_string - string saved with key 'info' in the pkl.
'''
if use_utm:
self.firenze.step()
armconfig1 = self.firenze.get_joint_angles('right_arm')
plist1, slist1 = self.scan_3d()
if use_camera:
cam_plist1, cam_imlist1 = self.image_region()
else:
cam_plist1, cam_imlist1 = None, None
rot_mat = tr.Rz(hook_angle - hook_3dprint_angle) * tr.Ry(
math.radians(-90))
if pull_loc == None:
self.pose_arm(hook_angle)
pull_loc = self.firenze.end_effector_pos('right_arm')
ut.save_pickle(
pull_loc,
'pull_loc_' + info_string + '_' + ut.formatted_time() + '.pkl')
else:
pt1 = copy.copy(pull_loc)
pt1[0, 0] = pt1[0, 0] - 0.1
print 'pt1:', pt1.A1
print 'pull_loc:', pull_loc.A1
self.firenze.go_cartesian('right_arm', pt1, rot_mat, speed=0.2)
self.firenze.go_cartesian('right_arm',
pull_loc,
rot_mat,
speed=0.07)
print 'press ENTER to pull'
k = m3t.get_keystroke()
if k != '\r':
return
time_dict = {}
time_dict['before_hook'] = time.time()
print 'first getting a good hook'
self.get_firm_hook(hook_angle)
time.sleep(0.5)
time_dict['before_pull'] = time.time()
print 'pull begins'
stiffness_scale = self.settings_r.stiffness_scale
vec = tr.rotX(-hook_angle) * np.matrix(
[0., 0.05 / stiffness_scale, 0.]).T
self.keep_hook_vec = vec
self.hook_maintain_dist_plane = np.dot(vec.A1, np.array([0., 1., 0.]))
self.eq_pt_cartesian = self.firenze.end_effector_pos('right_arm') + vec
q_eq = self.firenze.IK('right_arm', self.eq_pt_cartesian, rot_mat)
self.firenze.go_jointangles('right_arm', q_eq, speed=math.radians(30))
self.q_guess = q_eq
# self.q_guess = self.firenze.get_joint_angles('right_arm')
self.pull_trajectory = at.JointTrajectory()
self.jt_torque_trajectory = at.JointTrajectory()
self.eq_pt_trajectory = at.JointTrajectory()
self.force_trajectory = at.ForceTrajectory()
self.firenze.step()
start_config = self.firenze.get_joint_angles('right_arm')
self.eq_IK_rot_mat = rot_mat # for equi pt generators.
self.eq_force_threshold = force_threshold
self.hooked_location_moved = False # flag to indicate when the hooking location started moving.
self.prev_force_mag = np.linalg.norm(
self.firenze.get_wrist_force('right_arm'))
self.eq_motion_vec = np.matrix([-1., 0., 0.]).T
self.slip_count = 0
if strategy == 'line_neg_x':
result = self.compliant_motion(self.equi_pt_generator_line, 0.025)
elif strategy == 'control_radial_force':
self.cartesian_pts_list = []
self.piecewise_force_threshold = force_threshold
self.rad_guess = 1.0
self.cx = 0.6
self.cy = -self.rad_guess
self.start() # start the circle estimation thread
result = self.compliant_motion(
self.equi_pt_generator_control_radial_force, 0.025)
else:
raise RuntimeError('unknown pull strategy: ', strategy)
if result == 'slip: force step decrease' or result == 'danger of self collision':
self.firenze.motors_off()
print 'powered off the motors.'
print 'Compliant motion result:', result
print 'Original force threshold:', force_threshold
print 'Adapted force threshold:', self.eq_force_threshold
time_dict['after_pull'] = time.time()
d = {
'actual': self.pull_trajectory,
'eq_pt': self.eq_pt_trajectory,
'force': self.force_trajectory,
'torque': self.jt_torque_trajectory,
'stiffness': self.firenze.arm_settings['right_arm'],
'info': info_string,
'force_threshold': force_threshold,
'eq_force_threshold': self.eq_force_threshold,
'hook_angle': hook_angle,
'result': result,
'strategy': strategy,
'time_dict': time_dict
}
self.firenze.step()
armconfig2 = self.firenze.get_joint_angles('right_arm')
if use_utm:
plist2, slist2 = self.scan_3d()
d['start_config'] = start_config
d['armconfig1'] = armconfig1
d['armconfig2'] = armconfig2
d['l1'], d['l2'] = 0., -0.055
d['scanlist1'], d['poslist1'] = slist1, plist1
d['scanlist2'], d['poslist2'] = slist2, plist2
d['cam_plist1'] = cam_plist1
d['cam_imlist1'] = cam_imlist1
ut.save_pickle(
d, 'pull_trajectories_' + d['info'] + '_' + ut.formatted_time() +
'.pkl')
sys.exit()
if opt.la:
use_left_arm = True
use_right_arm = False
arm = 'left_arm'
else:
use_left_arm = False
use_right_arm = True
arm = 'right_arm'
cmg = CompliantMotionGenerator(move_segway_flag, use_right_arm,
use_left_arm)
print 'hit a key to power up the arms.'
k=m3t.get_keystroke()
cmg.firenze.power_on()
if reposition_flag:
rot_mat = tr.Rz(hook_angle)*tr.Ry(math.radians(-90))
cmg.firenze.pose_robot(arm,rot_mat)
hook_location = cmg.firenze.end_effector_pos(arm)
g = cmg.reposition_robot(hook_location)
cmg.firenze.go_cartesian(arm,g,rot_mat,speed=0.1)
if search_hook_flag:
hook_angle = math.radians(ha)
surface_angle = math.radians(0.)
p = np.matrix([0.45,-0.2,-0.23]).T
if arm == 'left_arm':
p[1,0] = p[1,0] * -1
def run_ik(step_delta, arm_name):
pose = get_pose(arm_name)
cmd = M3JointCmd()
cmd.chain = [0] * 7
cmd.control_mode = [0] * 7
cmd.smoothing_mode = [0] * 7
for i in range(7):
if arm_name == 'right_arm':
cmd.chain[i] = (int(M3Chain.RIGHT_ARM))
elif arm_name == 'left_arm':
cmd.chain[i] = (int(M3Chain.LEFT_ARM))
cmd.stiffness.append(stiffness)
cmd.position.append(pose[i])
cmd.velocity.append(10.0)
cmd.effort.append(0.0)
cmd.control_mode[i] = (int(mab.JOINT_MODE_ROS_THETA_GC))
cmd.smoothing_mode[i] = (int(mas.SMOOTHING_MODE_SLEW))
cmd.chain_idx.append(i)
cmd.header = Header(0, rospy.Time.now(), '0')
humanoid_pub.publish(cmd)
print 'Press any key to start IK demo.'
k = m3t.get_keystroke()
t = ik_thread(step_delta, arm_name)
t.start()
d = [0, 0, 0]
while 1:
print '-----------------------'
print 'Delta: ', t.delta
print '-----------------------'
print 'q: quit'
print '1: x+'
print '2: x-'
print '3: y+'
print '4: y-'
print '5: z+'
print '6: z-'
print 'space: step'
k = m3t.get_keystroke()
if k == 'q':
t.delta_done = True
return
if k == '1':
d[0] = d[0] + step_delta
if k == '2':
d[0] = d[0] - step_delta
if k == '3':
d[1] = d[1] + step_delta
if k == '4':
d[1] = d[1] - step_delta
if k == '5':
d[2] = d[2] + step_delta
if k == '6':
d[2] = d[2] - step_delta
t.set_delta(d)
print
print 'Error: ', t.aerror
print 'Target: ', t.target_pos
import time
proxy = m3p.M3RtProxy()
proxy.start()
name_ec = m3t.user_select_components_interactive(
proxy.get_available_components('m3actuator_ec'), single=True)
if name_ec is None:
exit()
comp = m3f.create_component(name_ec)
proxy.subscribe_status(comp)
proxy.make_operational(name_ec)
q_log = []
tq_log = []
print 'Ready to generate motion? Hit any key to start'
m3t.get_keystroke()
ts = time.time()
try:
while time.time() - ts > 5.0:
proxy.step()
q = comp.status.qei_on
tq = comp.status.adc_torque
time.sleep(0.25)
print 'DT', time.time() - ts, 'Q', q, 'TQ', tq
q_log.append(q)
tq_log.append(tq)
except (KeyboardInterrupt, EOFError):
proxy.stop()
poly, inv_poly = m3tc.get_polyfit_to_data(q_log, tq_log, n=1)
print 'Poly', poly
proxy = m3p.M3RtProxy()
proxy.start()
name_ec=m3t.user_select_components_interactive(proxy.get_available_components('m3actuator_ec'),single=True)
if name_ec is None:
exit()
comp=m3f.create_component(name_ec)
proxy.subscribe_status(comp)
proxy.make_operational(name_ec)
q_log=[]
tq_log=[]
print 'Ready to generate motion? Hit any key to start'
m3t.get_keystroke()
ts=time.time()
try:
while time.time()-ts>5.0:
proxy.step()
q=comp.status.qei_on
tq=comp.status.adc_torque
time.sleep(0.25)
print 'DT',time.time()-ts, 'Q',q,'TQ',tq
q_log.append(q)
tq_log.append(tq)
except (KeyboardInterrupt,EOFError):
proxy.stop()
poly,inv_poly=m3tc.get_polyfit_to_data(q_log,tq_log,n=1)
print 'Poly',poly
def start(self):
print '--------------------------'
print 'm: Target M3 arm'
print 'v: Target RVIZ'
print 'b: Target Both M3 and RVIZ'
print 'q: quit'
print '--------------'
print
self.k = 'a'
while self.k != 'm' and self.k != 'v' and self.k != 'b' and self.k != 'q':
self.k = m3t.get_keystroke()
if self.k == 'q':
return
self.proxy = m3p.M3RtProxy()
if self.k == 'm' or self.k == 'b':
self.proxy.start()
bot_name = m3t.get_robot_name()
if bot_name == "":
print 'Error: no robot components found:', bot_names
return
self.bot = m3f.create_component(bot_name)
arm_names = ['right_arm', 'left_arm']
self.arm_name = m3t.user_select_components_interactive(arm_names,
single=True)[0]
if self.arm_name == 'right_arm':
self.center = [0.450, -0.25, -0.1745]
else:
self.center = [0.450, 0.25, -0.1745]
avail_chains = self.bot.get_available_chains()
for c in avail_chains:
if c == 'torso':
self.center[2] += 0.5079
if self.k == 'v' or self.k == 'b':
self.viz = m3v.M3Viz(self.proxy, self.bot)
self.viz_launched = True
self.viz.turn_sim_on()
if self.k == 'v':
self.theta_0[:] = self.bot.get_theta_sim_deg(self.arm_name)[:]
if self.k == 'm' or self.k == 'b':
self.proxy.subscribe_status(self.bot)
self.proxy.publish_command(self.bot)
self.proxy.make_operational_all()
self.proxy.step()
self.theta_0[:] = self.bot.get_theta_deg(self.arm_name)[:]
self.m3_launched = True
self.theta_soln_deg = [0.] * self.bot.get_num_dof(self.arm_name)
self.thetadot_0 = [0.] * self.bot.get_num_dof(self.arm_name)
self.bot.set_slew_rate_proportion(self.arm_name, [1.0] * 7)
while True:
print '--------------------------'
print 'g: generate vias'
print 'd: display vias'
print 'v: set avg velocity (Current ', self.vel_avg, ')'
print 's: set stiffness (Current', self.stiffness, ')'
if self.k == 'b' or self.k == 'm':
print 'e: execute vias'
if self.k == 'b' or self.k == 'v':
print 't: test vias in visualizer'
print 'q: quit'
print '--------------'
print
m = m3t.get_keystroke()
if m == 'q':
return
if m == 'v':
print 'Enter avg velocity (0-60 Deg/S) [', self.vel_avg, ']'
self.vel_avg = max(0, min(60, m3t.get_float(self.vel_avg)))
if m == 's':
print 'Enter stiffness (0-1.0) [', self.stiffness, ']'
self.stiffness = max(0, min(1.0,
m3t.get_float(self.stiffness)))
if m == 'g':
self.vias = []
print
print '(s)quare or (c)ircle?'
shape = None
while shape != 's' and shape != 'c':
shape = m3t.get_keystroke()
length_m = 0.0
if shape == 's':
print
print 'Length of square side in cm (10-25) [25]'
length_cm = nu.float(max(10, min(25, m3t.get_int(25))))
length_m = length_cm / 100.0
diameter_m = 0.0
if shape == 'c':
print
print 'Diameter of circle in cm (10-25) [25]'
diameter_cm = nu.float(max(10, min(25, m3t.get_int(25))))
diameter_m = diameter_cm / 100.0
print
print 'Enter shape resolution (1-20 vias/side) [20]'
resolution = max(1, min(20, m3t.get_int(20)))
if self.m3_launched:
self.proxy.step()
x = self.center[0]
if shape == 's':
y_left = self.center[1] + length_m / 2.0
y_right = self.center[1] - length_m / 2.0
z_top = self.center[2] + length_m / 2.0
z_bottom = self.center[2] - length_m / 2.0
dy = (y_left - y_right) / nu.float(resolution)
dz = (z_top - z_bottom) / nu.float(resolution)
if self.arm_name == 'right_arm':
# first add start point
self.ik_vias.append([
x, y_left, z_top, self.axis_demo[0],
self.axis_demo[1], self.axis_demo[2]
])
# add top line
for i in range(resolution):
self.ik_vias.append([
x, y_left - (i + 1) * dy, z_top,
self.axis_demo[0], self.axis_demo[1],
self.axis_demo[2]
])
# add right line
for i in range(resolution):
self.ik_vias.append([
x, y_right, z_top - (i + 1) * dz,
self.axis_demo[0], self.axis_demo[1],
self.axis_demo[2]
])
# add bottom line
for i in range(resolution):
self.ik_vias.append([
x, y_right + (i + 1) * dy, z_bottom,
self.axis_demo[0], self.axis_demo[1],
self.axis_demo[2]
])
# add left line
for i in range(resolution):
self.ik_vias.append([
x, y_left, z_bottom + (i + 1) * dz,
self.axis_demo[0], self.axis_demo[1],
self.axis_demo[2]
])
else:
# first add start point
self.ik_vias.append([
x, y_right, z_top, self.axis_demo[0],
self.axis_demo[1], self.axis_demo[2]
])
# add top line
for i in range(resolution):
self.ik_vias.append([
x, y_right + (i + 1) * dy, z_top,
self.axis_demo[0], self.axis_demo[1],
self.axis_demo[2]
])
# add right line
for i in range(resolution):
self.ik_vias.append([
x, y_left, z_top - (i + 1) * dz,
self.axis_demo[0], self.axis_demo[1],
self.axis_demo[2]
])
# add bottom line
for i in range(resolution):
self.ik_vias.append([
x, y_left - (i + 1) * dy, z_bottom,
self.axis_demo[0], self.axis_demo[1],
self.axis_demo[2]
])
# add left line
for i in range(resolution):
self.ik_vias.append([
x, y_right, z_bottom + (i + 1) * dz,
self.axis_demo[0], self.axis_demo[1],
self.axis_demo[2]
])
if shape == 'c':
for i in range(resolution * 4 + 1):
dt = 2 * nu.pi / (nu.float(resolution) * 4)
t = (nu.pi / 2) + i * dt
if t > nu.pi:
t -= 2 * nu.pi
y = self.center[1] + (diameter_m / 2.0) * nu.cos(t)
z = self.center[2] + (diameter_m / 2.0) * nu.sin(t)
self.ik_vias.append([
x, y, z, self.axis_demo[0], self.axis_demo[1],
self.axis_demo[2]
])
self.vias.append(self.theta_0[:])
# use zero position as reference for IK solver
ref = [0] * self.bot.get_num_dof(self.arm_name)
# use holdup position as reference
ref = [30, 0, 0, 40, 0, 0, 0]
self.bot.set_theta_sim_deg(self.arm_name, ref)
for ikv in self.ik_vias:
theta_soln = []
print 'solving for ik via:', ikv
if self.bot.get_tool_axis_2_theta_deg_sim(
self.arm_name, ikv[:3], ikv[3:], theta_soln):
self.vias.append(theta_soln)
self.bot.set_theta_sim_deg(self.arm_name, theta_soln)
else:
print 'WARNING: no IK solution found for via ', ikv
self.bot.set_theta_sim_deg(self.arm_name, ref)
if self.viz_launched:
self.viz.step()
self.vias.append(self.theta_0[:])
if m == 'd':
print
print '--------- IK Vias (', len(self.ik_vias), ')--------'
print '---------------[end_xyz[3], end_axis[3]]-----------'
for ikv in self.ik_vias:
print ikv
print
print '--------- Joint Vias (', len(self.vias), ')--------'
for v in self.vias:
print v
if m == 'e' or m == 't':
if len(self.vias) != 0:
for v in self.vias:
#print 'Adding via',v
self.jt.add_via_deg(v, [self.vel_avg] * self.ndof)
self.jt.start(self.theta_0[:], self.thetadot_0[:])
print
print '--------- Splines (', len(
self.jt.splines), ')--------'
print '------------q_0, q_f, qdot_0, qdot_f, tf--------------'
for s in self.jt.splines:
print s.q_0, s.q_f, s.qdot_0, s.qdot_f, s.tf
print
print 'Hit any key to start or (q) to quit execution'
p = m3t.get_keystroke()
if p != 'q':
if self.m3_launched and m == 'e':
self.bot.set_motor_power_on()
self.bot.set_mode_theta_gc(self.arm_name)
self.bot.set_stiffness(
self.arm_name, [self.stiffness] *
self.bot.get_num_dof(self.arm_name))
while not self.jt.is_splined_traj_complete():
q = self.jt.step()
if self.viz_launched and m == 't':
self.bot.set_theta_sim_deg(self.arm_name, q)
self.viz.step()
time.sleep(0.1)
elif self.m3_launched and m == 'e':
self.bot.set_theta_deg(self.arm_name, q)
self.proxy.step()
self.ik_vias = []
def pull(self,hook_angle,force_threshold,use_utm=False,use_camera=False,strategy='line_neg_x',
pull_loc=None, info_string=''):
''' force_threshold - max force at which to stop pulling.
hook_angle - radians(0, -90, 90 etc.)
0 - horizontal, -pi/2 hook points up, +pi/2 hook points down
use_utm - to take 3D scans or not.
use_camera - to take pictures from the camera or not.
strategy - 'line_neg_x': move eq point along -x axis.
'piecewise_linear': try and estimate circle and move along it.
'control_radial_force': try and keep the radial force constant
'control_radial_dist'
pull_loc - 3x1 np matrix of location for pulling. If None then arm will go into
gravity comp and user can show the location.
info_string - string saved with key 'info' in the pkl.
'''
if use_utm:
self.firenze.step()
armconfig1 = self.firenze.get_joint_angles('right_arm')
plist1,slist1 = self.scan_3d()
if use_camera:
cam_plist1, cam_imlist1 = self.image_region()
else:
cam_plist1,cam_imlist1 = None,None
rot_mat = tr.Rz(hook_angle-hook_3dprint_angle)*tr.Ry(math.radians(-90))
if pull_loc == None:
self.pose_arm(hook_angle)
pull_loc = self.firenze.end_effector_pos('right_arm')
ut.save_pickle(pull_loc,'pull_loc_'+info_string+'_'+ut.formatted_time()+'.pkl')
else:
pt1 = copy.copy(pull_loc)
pt1[0,0] = pt1[0,0]-0.1
print 'pt1:', pt1.A1
print 'pull_loc:', pull_loc.A1
self.firenze.go_cartesian('right_arm',pt1,rot_mat,speed=0.2)
self.firenze.go_cartesian('right_arm',pull_loc,rot_mat,speed=0.07)
print 'press ENTER to pull'
k=m3t.get_keystroke()
if k != '\r':
return
time_dict = {}
time_dict['before_hook'] = time.time()
print 'first getting a good hook'
self.get_firm_hook(hook_angle)
time.sleep(0.5)
time_dict['before_pull'] = time.time()
print 'pull begins'
stiffness_scale = self.settings_r.stiffness_scale
vec = tr.rotX(-hook_angle) * np.matrix([0.,0.05/stiffness_scale,0.]).T
self.keep_hook_vec = vec
self.hook_maintain_dist_plane = np.dot(vec.A1,np.array([0.,1.,0.]))
self.eq_pt_cartesian = self.firenze.end_effector_pos('right_arm') + vec
q_eq = self.firenze.IK('right_arm',self.eq_pt_cartesian,rot_mat)
self.firenze.go_jointangles('right_arm',q_eq,speed=math.radians(30))
self.q_guess = q_eq
# self.q_guess = self.firenze.get_joint_angles('right_arm')
self.pull_trajectory = at.JointTrajectory()
self.jt_torque_trajectory = at.JointTrajectory()
self.eq_pt_trajectory = at.JointTrajectory()
self.force_trajectory = at.ForceTrajectory()
self.firenze.step()
start_config = self.firenze.get_joint_angles('right_arm')
self.eq_IK_rot_mat = rot_mat # for equi pt generators.
self.eq_force_threshold = force_threshold
self.hooked_location_moved = False # flag to indicate when the hooking location started moving.
self.prev_force_mag = np.linalg.norm(self.firenze.get_wrist_force('right_arm'))
self.eq_motion_vec = np.matrix([-1.,0.,0.]).T
self.slip_count = 0
if strategy == 'line_neg_x':
result = self.compliant_motion(self.equi_pt_generator_line,0.025)
elif strategy == 'control_radial_force':
self.cartesian_pts_list = []
self.piecewise_force_threshold = force_threshold
self.rad_guess = 1.0
self.cx = 0.6
self.cy = -self.rad_guess
self.start() # start the circle estimation thread
result = self.compliant_motion(self.equi_pt_generator_control_radial_force,0.025)
else:
raise RuntimeError('unknown pull strategy: ', strategy)
if result == 'slip: force step decrease' or result == 'danger of self collision':
self.firenze.motors_off()
print 'powered off the motors.'
print 'Compliant motion result:', result
print 'Original force threshold:', force_threshold
print 'Adapted force threshold:', self.eq_force_threshold
time_dict['after_pull'] = time.time()
d = {'actual': self.pull_trajectory, 'eq_pt': self.eq_pt_trajectory,
'force': self.force_trajectory, 'torque': self.jt_torque_trajectory,
'stiffness': self.firenze.arm_settings['right_arm'],
'info': info_string, 'force_threshold': force_threshold,
'eq_force_threshold': self.eq_force_threshold, 'hook_angle':hook_angle,
'result':result,'strategy':strategy,'time_dict':time_dict}
self.firenze.step()
armconfig2 = self.firenze.get_joint_angles('right_arm')
if use_utm:
plist2,slist2 = self.scan_3d()
d['start_config']=start_config
d['armconfig1']=armconfig1
d['armconfig2']=armconfig2
d['l1'],d['l2']=0.,-0.055
d['scanlist1'],d['poslist1']=slist1,plist1
d['scanlist2'],d['poslist2']=slist2,plist2
d['cam_plist1']=cam_plist1
d['cam_imlist1']=cam_imlist1
ut.save_pickle(d,'pull_trajectories_'+d['info']+'_'+ut.formatted_time()+'.pkl')
f = file(fn, 'r')
vias = yaml.safe_load(f.read())
print '-------------'
print len(vias), " vias loaded."
print '-------------'
k = 0
bot.set_slew_rate('right_arm',[5]*7)
bot.set_mode_theta_gc('right_arm')
bot.set_motor_power_on()
proxy.step()
while k < len(vias):
print "Hit <SPACE> to move to via ", k+1, " with positions: ", vias[k], " or <Q> to quit."
key = m3t.get_keystroke()
if key == ' ':
t = vias[k]
bot.set_theta_deg('right_arm', t)
proxy.step()
k += 1
if key == 'q':
print 'Exiting..'
break
bot.set_mode_off('right_arm')
bot.set_motor_power_off()
proxy.step()
proxy.stop()
def run_ik(step_delta, arm_name):
pose=get_pose(arm_name)
cmd = M3JointCmd()
cmd.chain = [0]*7
cmd.control_mode = [0]*7
cmd.smoothing_mode = [0]*7
for i in range(7):
if arm_name == 'right_arm':
cmd.chain[i] = (int(M3Chain.RIGHT_ARM))
elif arm_name == 'left_arm':
cmd.chain[i] = (int(M3Chain.LEFT_ARM))
cmd.stiffness.append(stiffness)
cmd.position.append(pose[i])
cmd.velocity.append(10.0)
cmd.effort.append(0.0)
cmd.control_mode[i] = (int(mab.JOINT_MODE_ROS_THETA_GC))
cmd.smoothing_mode[i] = (int(mas.SMOOTHING_MODE_SLEW))
cmd.chain_idx.append(i)
cmd.header = Header(0,rospy.Time.now(),'0')
humanoid_pub.publish(cmd)
print 'Press any key to start IK demo.'
k=m3t.get_keystroke()
t=ik_thread(step_delta, arm_name)
t.start()
d=[0,0,0]
while 1:
print '-----------------------'
print 'Delta: ',t.delta
print '-----------------------'
print 'q: quit'
print '1: x+'
print '2: x-'
print '3: y+'
print '4: y-'
print '5: z+'
print '6: z-'
print 'space: step'
k=m3t.get_keystroke()
if k=='q':
t.delta_done=True
return
if k=='1':
d[0]=d[0]+step_delta
if k=='2':
d[0]=d[0]-step_delta
if k=='3':
d[1]=d[1]+step_delta
if k=='4':
d[1]=d[1]-step_delta
if k=='5':
d[2]=d[2]+step_delta
if k=='6':
d[2]=d[2]-step_delta
t.set_delta(d)
print
print 'Error: ',t.aerror
print 'Target: ',t.target_pos
def start(self):
print '--------------------------'
print 'm: Target M3 arm'
print 'v: Target RVIZ'
print 'b: Target Both M3 and RVIZ'
print 'q: quit'
print '--------------'
print
self.k = 'a'
while self.k!='m' and self.k!='v' and self.k!='b' and self.k!='q':
self.k=m3t.get_keystroke()
if self.k=='q':
return
self.proxy = m3p.M3RtProxy()
if self.k=='m' or self.k=='b':
self.proxy.start()
bot_name=m3t.get_robot_name()
if bot_name == "":
print 'Error: no robot components found:', bot_names
return
self.bot=m3f.create_component(bot_name)
arm_names = ['right_arm', 'left_arm']
self.arm_name = m3t.user_select_components_interactive(arm_names,single=True)[0]
if self.arm_name == 'right_arm':
self.center = [0.450, -0.25, -0.1745]
else:
self.center = [0.450, 0.25, -0.1745]
avail_chains = self.bot.get_available_chains()
for c in avail_chains:
if c == 'torso':
self.center[2] += 0.5079
if self.k=='v' or self.k=='b':
self.viz = m3v.M3Viz(self.proxy, self.bot)
self.viz_launched = True
self.viz.turn_sim_on()
if self.k=='v':
self.theta_0[:] = self.bot.get_theta_sim_deg(self.arm_name)[:]
if self.k=='m' or self.k=='b':
self.proxy.subscribe_status(self.bot)
self.proxy.publish_command(self.bot)
self.proxy.make_operational_all()
self.proxy.step()
self.theta_0[:] = self.bot.get_theta_deg(self.arm_name)[:]
self.m3_launched = True
self.theta_soln_deg = [0.]*self.bot.get_num_dof(self.arm_name)
self.thetadot_0 = [0.]*self.bot.get_num_dof(self.arm_name)
self.bot.set_slew_rate_proportion(self.arm_name, [1.0]*7)
while True:
print '--------------------------'
print 'g: generate vias'
print 'd: display vias'
print 'v: set avg velocity (Current ',self.vel_avg,')'
print 's: set stiffness (Current',self.stiffness,')'
if self.k=='b' or self.k=='m':
print 'e: execute vias'
if self.k=='b' or self.k=='v':
print 't: test vias in visualizer'
print 'q: quit'
print '--------------'
print
m=m3t.get_keystroke()
if m=='q':
return
if m=='v':
print 'Enter avg velocity (0-60 Deg/S) [',self.vel_avg,']'
self.vel_avg=max(0,min(60,m3t.get_float(self.vel_avg)))
if m=='s':
print 'Enter stiffness (0-1.0) [',self.stiffness,']'
self.stiffness=max(0,min(1.0,m3t.get_float(self.stiffness)))
if m == 'g':
self.vias=[]
print
print '(s)quare or (c)ircle?'
shape = None
while shape != 's' and shape != 'c':
shape=m3t.get_keystroke()
length_m = 0.0
if shape == 's':
print
print 'Length of square side in cm (10-25) [25]'
length_cm = nu.float(max(10,min(25,m3t.get_int(25))))
length_m = length_cm / 100.0
diameter_m = 0.0
if shape == 'c':
print
print 'Diameter of circle in cm (10-25) [25]'
diameter_cm = nu.float(max(10,min(25,m3t.get_int(25))))
diameter_m = diameter_cm / 100.0
print
print 'Enter shape resolution (1-20 vias/side) [20]'
resolution = max(1,min(20,m3t.get_int(20)))
if self.m3_launched:
self.proxy.step()
x = self.center[0]
if shape == 's':
y_left = self.center[1] + length_m/2.0
y_right = self.center[1] - length_m/2.0
z_top = self.center[2] + length_m/2.0
z_bottom = self.center[2] - length_m/2.0
dy = (y_left - y_right) / nu.float(resolution)
dz = (z_top - z_bottom) / nu.float(resolution)
if self.arm_name=='right_arm':
# first add start point
self.ik_vias.append([x, y_left, z_top, self.axis_demo[0], self.axis_demo[1], self.axis_demo[2]])
# add top line
for i in range(resolution):
self.ik_vias.append([x, y_left - (i+1)*dy, z_top, self.axis_demo[0],self.axis_demo[1], self.axis_demo[2]])
# add right line
for i in range(resolution):
self.ik_vias.append([x, y_right, z_top - (i+1)*dz, self.axis_demo[0], self.axis_demo[1], self.axis_demo[2]])
# add bottom line
for i in range(resolution):
self.ik_vias.append([x, y_right + (i+1)*dy, z_bottom, self.axis_demo[0], self.axis_demo[1], self.axis_demo[2]])
# add left line
for i in range(resolution):
self.ik_vias.append([x, y_left, z_bottom + (i+1)*dz, self.axis_demo[0], self.axis_demo[1], self.axis_demo[2]])
else:
# first add start point
self.ik_vias.append([x, y_right, z_top, self.axis_demo[0], self.axis_demo[1], self.axis_demo[2]])
# add top line
for i in range(resolution):
self.ik_vias.append([x, y_right + (i+1)*dy, z_top, self.axis_demo[0],self.axis_demo[1], self.axis_demo[2]])
# add right line
for i in range(resolution):
self.ik_vias.append([x, y_left, z_top - (i+1)*dz, self.axis_demo[0], self.axis_demo[1], self.axis_demo[2]])
# add bottom line
for i in range(resolution):
self.ik_vias.append([x, y_left - (i+1)*dy, z_bottom, self.axis_demo[0], self.axis_demo[1], self.axis_demo[2]])
# add left line
for i in range(resolution):
self.ik_vias.append([x, y_right, z_bottom + (i+1)*dz, self.axis_demo[0], self.axis_demo[1], self.axis_demo[2]])
if shape == 'c':
for i in range(resolution*4 + 1):
dt = 2*nu.pi/(nu.float(resolution)*4)
t = (nu.pi/2) + i*dt
if t > nu.pi:
t -= 2*nu.pi
y = self.center[1] + (diameter_m/2.0) * nu.cos(t)
z = self.center[2] + (diameter_m/2.0) * nu.sin(t)
self.ik_vias.append([x, y, z, self.axis_demo[0], self.axis_demo[1], self.axis_demo[2]])
self.vias.append(self.theta_0[:])
# use zero position as reference for IK solver
ref=[0]*self.bot.get_num_dof(self.arm_name)
# use holdup position as reference
ref= [30,0,0,40,0,0,0]
self.bot.set_theta_sim_deg(self.arm_name,ref)
for ikv in self.ik_vias:
theta_soln = []
print 'solving for ik via:', ikv
if self.bot.get_tool_axis_2_theta_deg_sim(self.arm_name, ikv[:3], ikv[3:], theta_soln):
self.vias.append(theta_soln)
self.bot.set_theta_sim_deg(self.arm_name,theta_soln)
else:
print 'WARNING: no IK solution found for via ', ikv
self.bot.set_theta_sim_deg(self.arm_name,ref)
if self.viz_launched:
self.viz.step()
self.vias.append(self.theta_0[:])
if m=='d':
print
print '--------- IK Vias (', len(self.ik_vias), ')--------'
print '---------------[end_xyz[3], end_axis[3]]-----------'
for ikv in self.ik_vias:
print ikv
print
print '--------- Joint Vias (', len(self.vias), ')--------'
for v in self.vias:
print v
if m == 'e' or m=='t':
if len(self.vias) != 0:
for v in self.vias:
#print 'Adding via',v
self.jt.add_via_deg(v, [self.vel_avg]*self.ndof)
self.jt.start(self.theta_0[:], self.thetadot_0[:])
print
print '--------- Splines (', len(self.jt.splines), ')--------'
print '------------q_0, q_f, qdot_0, qdot_f, tf--------------'
for s in self.jt.splines:
print s.q_0, s.q_f, s.qdot_0, s.qdot_f, s.tf
print
print 'Hit any key to start or (q) to quit execution'
p=m3t.get_keystroke()
if p != 'q':
if self.m3_launched and m=='e':
self.bot.set_motor_power_on()
self.bot.set_mode_theta_gc(self.arm_name)
self.bot.set_stiffness(self.arm_name, [self.stiffness]*self.bot.get_num_dof(self.arm_name))
while not self.jt.is_splined_traj_complete():
q = self.jt.step()
if self.viz_launched and m=='t':
self.bot.set_theta_sim_deg(self.arm_name, q)
self.viz.step()
time.sleep(0.1)
elif self.m3_launched and m=='e':
self.bot.set_theta_deg(self.arm_name, q)
self.proxy.step()
self.ik_vias=[]
