def publishPostureGoal(joints, postureName, channel='POSTURE_GOAL'): ''' Given a dict mapping joint name strings to joint positions, creates a joint_angles_t LCM message and publishes the result on the given channel name. ''' msg = lcmbotcore.joint_angles_t() msg.utime = getUtime() for name, position in joints.iteritems(): msg.joint_name.append(name) msg.joint_position.append(position) msg.num_joints = len(msg.joint_name) lcmWrapper.publish(channel, msg) lcmWrapper.publish('POSTURE_GOAL_CANNED', msg) publishSystemStatus('sending posture goal: ' + postureName)
def publishPostureGoal(joints, postureName, channel="POSTURE_GOAL"): """ Given a dict mapping joint name strings to joint positions, creates a joint_angles_t LCM message and publishes the result on the given channel name. """ msg = lcmbotcore.joint_angles_t() msg.utime = getUtime() for name, position in joints.iteritems(): msg.joint_name.append(name) msg.joint_position.append(position) msg.num_joints = len(msg.joint_name) lcmWrapper.publish(channel, msg) lcmWrapper.publish("POSTURE_GOAL_CANNED", msg) publishSystemStatus("sending posture goal: " + postureName)
def setNeckPitch(neckPitchDegrees): assert neckPitchDegrees <= 90 and neckPitchDegrees >= -90 jointGroups = drcargs.getDirectorConfig()['teleopJointGroups'] jointGroupNeck = filter(lambda group: group['name'] == 'Neck', jointGroups) if (len(jointGroupNeck) == 1): neckJoints = jointGroupNeck[0]['joints'] else: return # Assume first neck joint is the pitch joint m = lcmbotcore.joint_angles_t() m.utime = getUtime() m.num_joints = 1 m.joint_name = [neckJoints[0]] m.joint_position = [math.radians(neckPitchDegrees)] lcmUtils.publish('DESIRED_NECK_ANGLES', m)
def setNeckPitch(neckPitchDegrees): assert neckPitchDegrees <= 90 and neckPitchDegrees >= -90 jointGroups = drcargs.getDirectorConfig()['teleopJointGroups'] jointGroupNeck = filter(lambda group: group['name'] == 'Neck', jointGroups) neckJoints = jointGroupNeck[0]['joints'] if (len(neckJoints) == 1): # Atlas jointPositions = [math.radians(neckPitchDegrees)] elif (len(neckJoints) == 3): # Valkyrie, assuming joint 0 is lowerNeckPitch jointPositions = [math.radians(neckPitchDegrees), 0, 0] else: return # Assume first neck joint is the pitch joint m = lcmbotcore.joint_angles_t() m.utime = getUtime() m.num_joints = len(neckJoints) m.joint_name = neckJoints m.joint_position = jointPositions lcmUtils.publish('DESIRED_NECK_ANGLES', m)
#!/usr/bin/env python import lcm import bot_core as lcmbotcore import time import sys import numpy as np myLCM = lcm.LCM() percentage_closed = float(sys.argv[1]) msg = lcmbotcore.joint_angles_t() msg.joint_name = [ "leftIndexFingerMotorPitch1", "leftMiddleFingerMotorPitch1", "leftPinkyMotorPitch1", "leftThumbMotorPitch1", "leftThumbMotorPitch2" ] #, "leftThumbMotorRoll"] msg.num_joints = len(msg.joint_name) grasp_map = np.array([1.0, 1.0, 1.0, 1.0, 1.0]) #, 1.0 ]) msg.joint_position = grasp_map * percentage_closed #msg.joint_position[5] = 1.0 myLCM.publish("DESIRED_HAND_ANGLES", msg.encode())
#!/usr/bin/env python import lcm import bot_core as lcmbotcore import time import sys myLCM = lcm.LCM() msg = lcmbotcore.joint_angles_t() msg.joint_name = ["lowerNeckPitch", "neckYaw", "upperNeckPitch"] msg.num_joints = len(msg.joint_name) msg.joint_position = [0] * len(msg.joint_name) msg.joint_position[0] = float(sys.argv[1]) msg.joint_position[1] = float(sys.argv[2]) msg.joint_position[2] = float(sys.argv[3]) myLCM.publish("DESIRED_NECK_ANGLES", msg.encode())