def run(self):
while not self.delta_done:
if self.update:
self.update = False
self.target_pos = self.target_pos_start + self.delta
qdes = []
qcurrent = []
if self.arm_name == 'right_arm':
qcurrent = right_arm_jnt_angles
else:
qcurrent = left_arm_jnt_angles
resp_ik = ik_srv(self.arm_name, self.target_pos[:],
self.target_rpy[:], qcurrent)
#print resp1.success, resp1.angles_solution
print 'new target_pos ', self.target_pos
print 'new target_rpy ', m3t.rad2deg(nu.array(self.target_rpy))
if resp_ik.success:
cmd = M3JointCmd()
cmd.chain = [0] * 7
cmd.control_mode = [0] * 7
cmd.smoothing_mode = [0] * 7
print 'new_q ', m3t.rad2deg(
nu.array(resp_ik.angles_solution))
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(resp_ik.angles_solution[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)
q = []
if arm_name == 'right_arm':
q = right_arm_jnt_angles
else:
q = left_arm_jnt_angles
resp_fk = fk_srv(arm_name, q)
self.aerror = nu.sqrt(
sum((nu.array(resp_fk.end_position) -
nu.array(resp_fk.end_rpy))**2))
time.sleep(0.1)
def run(self): while not self.delta_done: if self.update: self.update=False self.target_pos=self.target_pos_start+self.delta qdes=[] qcurrent = [] if self.arm_name == 'right_arm': qcurrent = right_arm_jnt_angles else: qcurrent = left_arm_jnt_angles resp_ik = ik_srv(self.arm_name,self.target_pos[:],self.target_rpy[:],qcurrent) #print resp1.success, resp1.angles_solution print 'new target_pos ', self.target_pos print 'new target_rpy ', m3t.rad2deg(nu.array(self.target_rpy)) if resp_ik.success: cmd = M3JointCmd() cmd.chain = [0]*7 cmd.control_mode = [0]*7 cmd.smoothing_mode = [0]*7 print 'new_q ', m3t.rad2deg(nu.array(resp_ik.angles_solution)) 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(resp_ik.angles_solution[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) q = [] if arm_name == 'right_arm': q = right_arm_jnt_angles else: q = left_arm_jnt_angles resp_fk = fk_srv(arm_name,q) self.aerror=nu.sqrt(sum((nu.array(resp_fk.end_position)-nu.array(resp_fk.end_rpy))**2)) time.sleep(0.1)
def __init__ (self,step_delta,arm_name): Thread.__init__(self) self.arm_name = arm_name self.verror=0 self.aerror=0 self.delta_done=False self.delta=nu.zeros(3) self.target_pos=nu.zeros(3) q = [] if arm_name == 'right_arm': q = right_arm_jnt_angles else: q = left_arm_jnt_angles resp_fk = fk_srv(arm_name,q) self.target_pos_start = resp_fk.end_position print 'target_pos_start', self.target_pos_start self.target_rpy = resp_fk.end_rpy print 'target_rpy', m3t.rad2deg(nu.array(self.target_rpy)) self.step_delta=step_delta #self.update=True self.update=False
def __init__(self, step_delta, arm_name):
Thread.__init__(self)
self.arm_name = arm_name
self.verror = 0
self.aerror = 0
self.delta_done = False
self.delta = nu.zeros(3)
self.target_pos = nu.zeros(3)
q = []
if arm_name == 'right_arm':
q = right_arm_jnt_angles
else:
q = left_arm_jnt_angles
resp_fk = fk_srv(arm_name, q)
self.target_pos_start = resp_fk.end_position
print 'target_pos_start', self.target_pos_start
self.target_rpy = resp_fk.end_rpy
print 'target_rpy', m3t.rad2deg(nu.array(self.target_rpy))
self.step_delta = step_delta
#self.update=True
self.update = False
