def on_manip_plan(channel, data):
m = robot_plan_t.decode(data)
print "got manip plan"
print m.plan[m.num_states-1].utime
s.status.last_plan_msg_utime = s.last_utime
s.status.last_plan_start_utime = s.last_utime
s.manip_until_utime = s.last_utime + m.plan[m.num_states-1].utime
def on_manip_plan(channel, data):
m = robot_plan_t.decode(data)
print "got manip plan"
print m.plan[m.num_states - 1].utime
s.status.last_plan_msg_utime = s.last_utime
s.status.last_plan_start_utime = s.last_utime
s.manip_until_utime = s.last_utime + m.plan[m.num_states - 1].utime
def on_manip_plan(channel, data):
# This offset is paired with planDesiredOffset in lcm2ros_ihmc.cpp
planDesiredOffset = 1E6
m = robot_plan_t.decode(data)
print "got manip plan of length ", m.plan[m.num_states - 1].utime * 1E-6
print "adding planDesiredOffset of ", planDesiredOffset * 1E-6, " seconds here"
s.status.last_plan_msg_utime = s.last_utime
s.status.last_plan_start_utime = s.last_utime
s.manip_until_utime = s.last_utime + m.plan[m.num_states -
1].utime + planDesiredOffset
def on_manip_plan(channel, data):
m = robot_plan_t.decode(data)
print "got plan msg at" , s.last_utime , "with" , m.num_states , "states"
#print m.plan[m.num_states-1].utime
s.last_committed_plan = m
def on_plan(channel, data):
global atlas_version
m = robot_plan_t.decode(data)
estate = m.plan[m.num_states - 1]
estate_time = estate.utime * 1E-6
print estate_time
if ('LeftShoulderExtensor' in estate.joint_name):
print('received plan for valkyrie')
l_arm_names = [
'LeftShoulderExtensor', 'LeftShoulderAdductor',
'LeftShoulderSupinator', 'LeftElbowExtensor',
'LeftForearmSupinator', 'LeftWristExtensor', 'LeftWrist'
]
r_arm_names = [
'RightShoulderExtensor', 'RightShoulderAdductor',
'RightShoulderSupinator', 'RightElbowExtensor',
'RightForearmSupinator', 'RightWristExtensor', 'RightWrist'
]
else:
regex = re.compile('l_arm*')
l_arm_names = [
string for string in estate.joint_name if re.match(regex, string)
]
regex = re.compile('r_arm*')
r_arm_names = [
string for string in estate.joint_name if re.match(regex, string)
]
print 'sending', len(l_arm_names), 'left and', len(
r_arm_names), 'right joints'
l_arm_angles = []
for i in range(0, len(l_arm_names)):
ix = estate.joint_name.index(l_arm_names[i])
val = estate.joint_position[ix]
l_arm_angles.extend([val])
r_arm_angles = []
for i in range(0, len(r_arm_names)):
ix = estate.joint_name.index(r_arm_names[i])
val = estate.joint_position[ix]
r_arm_angles.extend([val])
#print l_arm_angles
msg = hand_pose_packet_message_t()
msg.utime = m.utime
msg.reference_frame = 0 # not used with joints
msg.position = [0, 0, 0] # ee position and orientation
msg.orientation = [1, 0, 0, 0]
msg.trajectory_time = estate_time
msg.data_type = 1 # ee pos=0 | joint angles =1
msg.robot_side = 0
msg.to_home_position = False
msg.n_angles = len(l_arm_angles)
msg.joint_angles = l_arm_angles
lc.publish("VAL_COMMAND_HAND_POSE", msg.encode())
print "Sending through left1"
time.sleep(1)
msg.robot_side = 1
msg.to_home_position = False
msg.n_angles = len(r_arm_angles)
msg.joint_angles = r_arm_angles
lc.publish("VAL_COMMAND_HAND_POSE", msg.encode())
print "Sending through plan right1"
msg.robot_side = 0
msg.to_home_position = False
msg.n_angles = len(l_arm_angles)
msg.joint_angles = l_arm_angles
lc.publish("VAL_COMMAND_HAND_POSE", msg.encode())
print "Sending through left2"
time.sleep(1)
msg.robot_side = 1
msg.to_home_position = False
msg.n_angles = len(r_arm_angles)
msg.joint_angles = r_arm_angles
lc.publish("VAL_COMMAND_HAND_POSE", msg.encode())
print "Sending through plan right2"
def on_plan(channel, data):
global atlas_version
m = robot_plan_t.decode(data)
estate = m.plan[m.num_states-1]
estate_time = estate.utime*1E-6
print estate_time
if ('LeftShoulderExtensor' in estate.joint_name):
print('received plan for valkyrie')
l_arm_names = ['LeftShoulderExtensor','LeftShoulderAdductor','LeftShoulderSupinator','LeftElbowExtensor','LeftForearmSupinator','LeftWristExtensor','LeftWrist']
r_arm_names = ['RightShoulderExtensor','RightShoulderAdductor','RightShoulderSupinator','RightElbowExtensor','RightForearmSupinator','RightWristExtensor','RightWrist']
else:
regex = re.compile('l_arm*')
l_arm_names = [string for string in estate.joint_name if re.match(regex,string)]
regex = re.compile('r_arm*')
r_arm_names = [string for string in estate.joint_name if re.match(regex,string)]
print 'sending' , len(l_arm_names) , 'left and' , len(r_arm_names) , 'right joints'
l_arm_angles = []
for i in range(0,len(l_arm_names)):
ix= estate.joint_name.index(l_arm_names[i])
val = estate.joint_position[ix]
l_arm_angles.extend([val])
r_arm_angles = []
for i in range(0,len(r_arm_names)):
ix= estate.joint_name.index(r_arm_names[i])
val = estate.joint_position[ix]
r_arm_angles.extend([val])
#print l_arm_angles
msg = hand_pose_packet_message_t()
msg.utime = m.utime
msg.reference_frame = 0 # not used with joints
msg.position = [0,0,0] # ee position and orientation
msg.orientation = [1,0,0,0]
msg.trajectory_time = estate_time
msg.data_type = 1 # ee pos=0 | joint angles =1
msg.robot_side = 0
msg.to_home_position = False
msg.n_angles = len(l_arm_angles)
msg.joint_angles = l_arm_angles
lc.publish("VAL_COMMAND_HAND_POSE", msg.encode())
print "Sending through left1"
time.sleep(1)
msg.robot_side = 1
msg.to_home_position = False
msg.n_angles = len(r_arm_angles)
msg.joint_angles = r_arm_angles
lc.publish("VAL_COMMAND_HAND_POSE", msg.encode())
print "Sending through plan right1"
msg.robot_side = 0
msg.to_home_position = False
msg.n_angles = len(l_arm_angles)
msg.joint_angles = l_arm_angles
lc.publish("VAL_COMMAND_HAND_POSE", msg.encode())
print "Sending through left2"
time.sleep(1)
msg.robot_side = 1
msg.to_home_position = False
msg.n_angles = len(r_arm_angles)
msg.joint_angles = r_arm_angles
lc.publish("VAL_COMMAND_HAND_POSE", msg.encode())
print "Sending through plan right2"
