Python robot_state_tの例

コード例 #1

ファイル: drc_utils.py プロジェクト: LCM-projects/drc_core_utils

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  

コード例 #2

ファイル: send_robot_state.py プロジェクト: Gastd/oh-distro

    "LeftHipAdductor",    "LeftHipExtensor",    "LeftKneeExtensor",    "LeftAnkleExtensor",    "LeftAnkle",    
    "RightHipRotator",   "RightHipAdductor",    "RightHipExtensor",    "RightKneeExtensor",    "RightAnkleExtensor",    "RightAnkle"]
elif robot_name == "val2":
  print "Valkyrie Version 2"
  joint_names = ["torsoYaw",    "torsoPitch",    "torsoRoll",    "lowerNeckPitch",    "neckYaw",    "upperNeckPitch",
    "rightShoulderPitch",     "rightShoulderRoll",    "rightShoulderYaw",    "rightElbowPitch",    "rightForearmYaw",    "rightWristRoll",
    "rightWristPitch",    "leftShoulderPitch",    "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"]

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

コード例 #3

#!/usr/bin/python
import os, sys
import time

home_dir = os.getenv("HOME")
#print home_dir
sys.path.append(home_dir + "/drc/software/build/lib/python2.7/site-packages")
sys.path.append(home_dir + "/drc/software/build/lib/python2.7/dist-packages")

import lcm
from drc.robot_state_t import robot_state_t
import time

lc = lcm.LCM()
print "Send Val..."
msg = robot_state_t()

msg.utime = 0
msg.joint_name = [
    'back_bkz', 'back_bky', 'back_bkx', 'neck_ay', 'neck_by', 'neck_cy',
    'r_arm_shz', 'r_arm_shx', 'r_arm_ely', 'r_arm_elx', 'r_arm_uwy',
    'r_arm_mwx', 'r_arm_lwy', 'l_arm_shz', 'l_arm_shx', 'l_arm_ely',
    'l_arm_elx', 'l_arm_uwy', 'l_arm_mwx', 'l_arm_lwy', 'r_leg_hpz',
    'r_leg_hpx', 'r_leg_hpy', 'r_leg_kny', 'r_leg_aky', 'r_leg_akx',
    'l_leg_hpz', 'l_leg_hpx', 'l_leg_hpy', 'l_leg_kny', 'l_leg_aky',
    'l_leg_akx'
]

msg.num_joints = len(msg.joint_name)

msg.joint_position = [0] * msg.num_joints