def on_pose_body(channel, data):
if (skip_joint_angles is False):
if (received_joint_state is False):
return
m = pose_t.decode(data)
o = robot_state_t()
o.utime = m.utime
o.num_joints = joint_state.num_joints
o.joint_name = joint_state.joint_name
o.joint_position = joint_state.joint_position
o.joint_velocity = joint_state.joint_velocity
o.joint_effort = joint_state.joint_effort
o.pose.translation.x =m.pos[0];
o.pose.translation.y =m.pos[1];
o.pose.translation.z =m.pos[2];
o.pose.rotation.w = m.orientation[0];
o.pose.rotation.x = m.orientation[1];
o.pose.rotation.y = m.orientation[2];
o.pose.rotation.z = m.orientation[3];
lc.publish("EST_ROBOT_STATE",o.encode())
def send_state():
o = robot_state_t()
o.utime = timestamp_now()
o.num_joints = len(joint_name_list)
o.joint_name = joint_name_list
o.joint_position = random.rand(o.num_joints) - 0.25
o.joint_velocity = [0] * o.num_joints
o.joint_effort = [0] * o.num_joints
o.pose.translation.x = 0
o.pose.translation.y = 0
o.pose.translation.z = 1
o.pose.rotation.w = 1
o.pose.rotation.x = 0
o.pose.rotation.y = 0
o.pose.rotation.z = 0
lc.publish("EST_ROBOT_STATE_ALT", o.encode())
def getRobotStateMsg():
msg = robot_state_t()
msg.utime = timestamp_now ()
ft = force_torque_t()
msg.pose = getPoseMsg()
msg.twist = getTwistMsg()
msg.force_torque = ft
msg.num_joints = 28
msg.joint_position = [0]*28
msg.joint_velocity = [0]*28
msg.joint_effort = [0]*28
msg.joint_name = ['back_bkz', 'back_bky', 'back_bkx', 'neck_ay', 'l_leg_hpz', 'l_leg_hpx',\
'l_leg_hpy', 'l_leg_kny', 'l_leg_aky', 'l_leg_akx', 'r_leg_hpz', 'r_leg_hpx',\
'r_leg_hpy', 'r_leg_kny', 'r_leg_aky', 'r_leg_akx', 'l_arm_usy', 'l_arm_shx',\
'l_arm_ely', 'l_arm_elx', 'l_arm_uwy', 'l_arm_mwx', 'r_arm_usy', 'r_arm_shx',\
'r_arm_ely', 'r_arm_elx', 'r_arm_uwy', 'r_arm_mwx']
return msg
def send_state():
o = robot_state_t()
o.utime = timestamp_now ()
o.num_joints = len(joint_name_list)
o.joint_name = joint_name_list
o.joint_position = random.rand(o.num_joints) -0.25
o.joint_velocity = [0]*o.num_joints
o.joint_effort = [0]*o.num_joints
o.pose.translation.x =0;
o.pose.translation.y =0;
o.pose.translation.z =1;
o.pose.rotation.w = 1;
o.pose.rotation.x = 0
o.pose.rotation.y = 0
o.pose.rotation.z = 0
lc.publish("EST_ROBOT_STATE_ALT",o.encode())
def on_pose_body(channel, data):
global core_robot_state, joint_names
if (core_robot_state.num_joints==0):
return
m = pose_t.decode(data)
o = robot_state_t()
o.utime = m.utime
o.num_joints = core_robot_state.num_joints
o.joint_name = core_robot_state.joint_name
o.joint_position = core_robot_state.joint_position
o.joint_velocity = core_robot_state.joint_velocity
o.joint_effort = core_robot_state.joint_effort
nrot = quaternion_t()
nvec = vector_3d_t()
p = position_3d_t()
p.rotation = nrot
p.translation = nvec
o.pose = p
t = twist_t()
t.linear_velocity = nvec
t.angular_velocity = nvec
o.twist = t
o.pose.translation.x =m.pos[0];
o.pose.translation.y =m.pos[1];
o.pose.translation.z =m.pos[2];
o.pose.rotation.w = m.orientation[0];
o.pose.rotation.x = m.orientation[1];
o.pose.rotation.y = m.orientation[2];
o.pose.rotation.z = m.orientation[3];
ft = force_torque_t()
o.force_torque = ft
lc.publish("EST_ROBOT_STATE",o.encode())
def send_state():
o = robot_state_t()
o.utime = timestamp_now()
o.num_joints = len(joint_name_list)
#o.joint_name = ["" for x in range(o.num_joints)]
o.joint_name = joint_name_list
o.joint_position = random.rand(o.num_joints) - 0.5
#o.joint_position = [0.0, 0.0, -1.3962634015945001, 0.0, 0.0, -1.3962634015945001, 0.0, -2.220446049250313e-16, 1.3962634015945001, 0.0, -2.220446049250313e-16, 1.3962634015945001, 0.0, 1.1102230246251565e-16, 0,0]
o.joint_velocity = [0] * o.num_joints
o.joint_effort = [0] * o.num_joints
o.pose.translation.x = 0
o.pose.translation.y = 0
o.pose.translation.z = 2.3
o.pose.rotation.w = 1
o.pose.rotation.x = 0
o.pose.rotation.y = 0
o.pose.rotation.z = 0
lc.publish("EST_ROBOT_STATE", o.encode())
def on_pose_body(channel, data):
global core_robot_state, joint_names
if (core_robot_state.num_joints == 0):
return
m = pose_t.decode(data)
o = robot_state_t()
o.utime = m.utime
o.num_joints = core_robot_state.num_joints
o.joint_name = core_robot_state.joint_name
o.joint_position = core_robot_state.joint_position
o.joint_velocity = core_robot_state.joint_velocity
o.joint_effort = core_robot_state.joint_effort
nrot = quaternion_t()
nvec = vector_3d_t()
p = position_3d_t()
p.rotation = nrot
p.translation = nvec
o.pose = p
t = twist_t()
t.linear_velocity = nvec
t.angular_velocity = nvec
o.twist = t
o.pose.translation.x = m.pos[0]
o.pose.translation.y = m.pos[1]
o.pose.translation.z = m.pos[2]
o.pose.rotation.w = m.orientation[0]
o.pose.rotation.x = m.orientation[1]
o.pose.rotation.y = m.orientation[2]
o.pose.rotation.z = m.orientation[3]
ft = force_torque_t()
o.force_torque = ft
lc.publish("EST_ROBOT_STATE", o.encode())
def on_core_robot_state(channel, data):
core_robot_state = joint_state_t.decode(data)
#m = pose_t.decode(data)
o = robot_state_t()
o.utime = core_robot_state.utime
o.num_joints = core_robot_state.num_joints
o.joint_name = core_robot_state.joint_name
o.joint_position = core_robot_state.joint_position
o.joint_velocity = core_robot_state.joint_velocity
o.joint_effort = core_robot_state.joint_effort
nrot = quaternion_t()
nvec = vector_3d_t()
p = position_3d_t()
p.rotation = nrot
p.translation = nvec
o.pose = p
t = twist_t()
t.linear_velocity = nvec
t.angular_velocity = nvec
o.twist = t
o.pose.translation.x = 0
o.pose.translation.y = 0
o.pose.translation.z = 0
o.pose.rotation.w = 1
o.pose.rotation.x = 0
o.pose.rotation.y = 0
o.pose.rotation.z = 0
ft = force_torque_t()
o.force_torque = ft
lc.publish("EST_ROBOT_STATE", o.encode())
def on_pose_body(channel, data):
if (joint_state.num_joints==0):
return
m = pose_t.decode(data)
o = robot_state_t()
o.utime = m.utime
o.num_joints = joint_state.num_joints
#o.joint_name = ["" for x in range(o.num_joints)]
o.joint_name = joint_state.joint_name
o.joint_position = joint_state.joint_position
o.joint_velocity = joint_state.joint_velocity
o.joint_effort = joint_state.joint_effort
o.pose.translation.x =m.pos[0];
o.pose.translation.y =m.pos[1];
o.pose.translation.z =m.pos[2];
o.pose.rotation.w = m.orientation[0];
o.pose.rotation.x = m.orientation[1];
o.pose.rotation.y = m.orientation[2];
o.pose.rotation.z = m.orientation[3];
lc.publish("EST_ROBOT_STATE",o.encode())
"leftShoulderRoll", "leftShoulderYaw", "leftElbowPitch",
"leftForearmYaw", "leftWristRoll", "leftWristPitch", "leftHipYaw",
"leftHipRoll", "leftHipPitch", "leftKneePitch", "leftAnklePitch",
"leftAnkleRoll", "rightHipYaw", "rightHipRoll", "rightHipPitch",
"rightKneePitch", "rightAnklePitch", "rightAnkleRoll"
]
elif robot_name == "multisense":
print "Multisense"
joint_names = ["hokuyo_joint"]
elif robot_name == "husky":
print "Husky"
joint_names = ["hokuyo_joint"]
lc = lcm.LCM()
print "Sending EST_ROBOT_STATE for ", robot_name
msg = robot_state_t()
msg.utime = 0
msg.joint_name = joint_names
msg.num_joints = len(msg.joint_name)
msg.joint_position = [0] * msg.num_joints
msg.joint_velocity = [0] * msg.num_joints
msg.joint_effort = [0] * msg.num_joints
msg2 = pose_t()
msg2.utime = msg.utime
if mode == "static":
print "Send a single pose..."
msg.pose.translation.x = 0
msg.pose.translation.y = 0
msg.pose.translation.z = 1.2
