def convertLCM_Matlab(x):
NUM_JOINT = 40
msg = hubo_hubo2state()
msg.timestamp = time.time()
msg.state = [0,0,0,0,0,0] #Basic Link Position
msg.state += [x.joint[i].pos for i in range(NUM_JOINT)] #Moter joints positions
msg.state += [0,0,0,0,0,0] #Basic Link Velocities
msg.state += [x.joint[i].vel for i in range(NUM_JOINT)] #Motor joint velocity
#Retroactively adding passive finger joints and their velocity
FINGER_JOINTPOS = [17,38] #Left and Right hand.
FINGER_JOINTPOS = [[FINGER_JOINTPOS[hand] +2*finger for finger in range(5) ] for hand in range(2)]#Populate 5 fingers
FINGER_JOINTPOS = FINGER_JOINTPOS[0] + FINGER_JOINTPOS[1]
FINGER_JOINTPOS += [FINGER_JOINTPOS[finger] +1 for finger in range(len(FINGER_JOINTPOS))]#Populate 2 knuckles for each finger
FINGER_JOINTPOS += [2*knuckles for knuckles in FINGER_JOINTPOS] #add velocity
FINGER_JOINTPOS.sort()
[msg.state.insert(knuckle, 0) for knuckle in FINGER_JOINTPOS] #Insert zero for the passive knuckles position + Velocity
return msg
def convertLCM_Matlab(x):
msg = hubo_hubo2state()
msg.timestamp = time.time()
#Baselink state not included
msg.NeckYaw = x.joint[ha.NKY].pos
msg.NeckYawdot = x.joint[ha.NKY].vel
#
# msg.HNP = x.joint[ha.HNP].pos
# msg.HNPdot = x.joint[ha.HNP].vel
#
# msg.HNR = x.joint[ha.HNR].pos
# msg.HNRdot = x.joint[ha.HNR].vel
#
msg.LeftShoulderPitch = x.joint[ha.LSP].pos
msg.LeftShoulderPitchdot = x.joint[ha.LSP].vel
msg.LeftShoulderRoll = x.joint[ha.LSR].pos
msg.LeftShoulderRolldot = x.joint[ha.LSR].vel
msg.LeftShoulderYaw = x.joint[ha.LSY].pos
msg.LeftShoulderYawdot = x.joint[ha.LSY].vel
msg.LeftElbowPitch = x.joint[ha.LEB].pos
msg.LeftElbowPitchdot = x.joint[ha.LEB].vel
msg.LeftWristYaw = x.joint[ha.LWY].pos
msg.LeftWristYawdot = x.joint[ha.LWY].vel
msg.LeftWristPitch = x.joint[ha.LWP].pos
msg.LeftWristPitchdot = x.joint[ha.LWP].vel
msg.RightShoulderPitch = x.joint[ha.RSP].pos
msg.RightShoulderPitchdot = x.joint[ha.RSP].vel
msg.RightShoulderRoll = x.joint[ha.RSR].pos
msg.RightShoulderRolldot = x.joint[ha.RSR].vel
msg.RightShoulderYaw = x.joint[ha.RSY].pos
msg.RightShoulderYawdot = x.joint[ha.RSY].vel
msg.RightElbowPitch = x.joint[ha.REB].pos
msg.RightElbowPitchdot = x.joint[ha.REB].vel
msg.RightWristYaw = x.joint[ha.RWY].pos
msg.RightWristYawdot = x.joint[ha.RWY].vel
msg.RightWristPitch = x.joint[ha.RWP].pos
msg.RightWristPitchdot = x.joint[ha.RWP].vel
msg.LeftFinger1 = x.joint[ha.LF1].pos
msg.LeftFinger1dot = x.joint[ha.LF1].vel
msg.LeftFinger2 = x.joint[ha.LF2].pos
msg.LeftFinger2dot = x.joint[ha.LF2].vel
msg.LeftFinger3 = x.joint[ha.LF3].pos
msg.LeftFinger3dot = x.joint[ha.LF3].vel
msg.LeftFinger4 = x.joint[ha.LF4].pos
msg.LeftFinger4dot = x.joint[ha.LF4].vel
msg.LeftFinger5 = x.joint[ha.LF5].pos
msg.LeftFinger5dot = x.joint[ha.LF5].vel
msg.RightFinger1 = x.joint[ha.RF1].pos
msg.RightFinger1dot = x.joint[ha.RF1].vel
msg.RightFinger2 = x.joint[ha.RF2].pos
msg.RightFinger2dot = x.joint[ha.RF2].vel
msg.RightFinger3 = x.joint[ha.RF3].pos
msg.RightFinger3dot = x.joint[ha.RF3].vel
msg.RightFinger4 = x.joint[ha.RF4].pos
msg.RightFinger4dot = x.joint[ha.RF4].vel
msg.RightFinger5 = x.joint[ha.RF5].pos
msg.RightFinger5dot = x.joint[ha.RF5].vel
msg.TrunkYaw = x.joint[ha.WST].pos
msg.TrunkYawdot = x.joint[ha.WST].vel
msg.LeftHipYaw = x.joint[ha.LHY].pos
msg.LeftHipYawdot = x.joint[ha.LHY].vel
msg.LeftHipRoll = x.joint[ha.LHR].pos
msg.LeftHipRolldot = x.joint[ha.LHR].vel
msg.LeftHipPitch = x.joint[ha.LHP].pos
msg.LeftHipPitchdot = x.joint[ha.LHP].vel
msg.LeftKneePitch = x.joint[ha.LKN].pos
msg.LeftKneePitchdot = x.joint[ha.LKN].vel
msg.LeftAnklePitch = x.joint[ha.LAP].pos
msg.LeftAnklePitchdot = x.joint[ha.LAP].vel
msg.LeftAnkleRoll = x.joint[ha.LAR].pos
msg.LeftAnkleRolldot = x.joint[ha.LAR].vel
msg.RightHipYaw = x.joint[ha.RHY].pos
msg.RightHipYawdot = x.joint[ha.RHY].vel
msg.RightHipRoll = x.joint[ha.RHR].pos
msg.RightHipRolldot = x.joint[ha.RHR].vel
msg.RightHipPitch = x.joint[ha.RHP].pos
msg.RightHipPitchdot = x.joint[ha.RHP].vel
msg.RightKneePitch = x.joint[ha.RKN].pos
msg.RightKneePitchdot = x.joint[ha.RKN].vel
msg.RightAnklePitch = x.joint[ha.RAP].pos
msg.RightAnklePitchdot = x.joint[ha.RAP].vel
msg.RightAnkleRoll = x.joint[ha.RAR].pos
msg.RightAnkleRolldot = x.joint[ha.RAR].vel
return msg
def convertLCM_Matlab(x):
msg = hubo_hubo2state()
msg.timestamp = time.time()
#Baselink state not included
msg.NeckYaw = x.joint[ha.NKY].pos
msg.NeckYawdot = x.joint[ha.NKY].vel
#
# msg.HNP = x.joint[ha.HNP].pos
# msg.HNPdot = x.joint[ha.HNP].vel
#
# msg.HNR = x.joint[ha.HNR].pos
# msg.HNRdot = x.joint[ha.HNR].vel
#
msg.LeftShoulderPitch = x.joint[ha.LSP].pos
msg.LeftShoulderPitchdot = x.joint[ha.LSP].vel
msg.LeftShoulderRoll = x.joint[ha.LSR].pos
msg.LeftShoulderRolldot = x.joint[ha.LSR].vel
msg.LeftShoulderYaw = x.joint[ha.LSY].pos
msg.LeftShoulderYawdot = x.joint[ha.LSY].vel
msg.LeftElbowPitch = x.joint[ha.LEB].pos
msg.LeftElbowPitchdot = x.joint[ha.LEB].vel
msg.LeftWristYaw = x.joint[ha.LWY].pos
msg.LeftWristYawdot = x.joint[ha.LWY].vel
msg.LeftWristPitch = x.joint[ha.LWP].pos
msg.LeftWristPitchdot = x.joint[ha.LWP].vel
msg.RightShoulderPitch = x.joint[ha.RSP].pos
msg.RightShoulderPitchdot = x.joint[ha.RSP].vel
msg.RightShoulderRoll = x.joint[ha.RSR].pos
msg.RightShoulderRolldot = x.joint[ha.RSR].vel
msg.RightShoulderYaw = x.joint[ha.RSY].pos
msg.RightShoulderYawdot = x.joint[ha.RSY].vel
msg.RightElbowPitch = x.joint[ha.REB].pos
msg.RightElbowPitchdot = x.joint[ha.REB].vel
msg.RightWristYaw = x.joint[ha.RWY].pos
msg.RightWristYawdot = x.joint[ha.RWY].vel
msg.RightWristPitch = x.joint[ha.RWP].pos
msg.RightWristPitchdot = x.joint[ha.RWP].vel
msg.LeftFinger1 = x.joint[ha.LF1].pos
msg.LeftFinger1dot = x.joint[ha.LF1].vel
msg.LeftFinger2 = x.joint[ha.LF2].pos
msg.LeftFinger2dot = x.joint[ha.LF2].vel
msg.LeftFinger3 = x.joint[ha.LF3].pos
msg.LeftFinger3dot = x.joint[ha.LF3].vel
msg.LeftFinger4 = x.joint[ha.LF4].pos
msg.LeftFinger4dot = x.joint[ha.LF4].vel
msg.LeftFinger5 = x.joint[ha.LF5].pos
msg.LeftFinger5dot = x.joint[ha.LF5].vel
msg.RightFinger1 = x.joint[ha.RF1].pos
msg.RightFinger1dot = x.joint[ha.RF1].vel
msg.RightFinger2 = x.joint[ha.RF2].pos
msg.RightFinger2dot = x.joint[ha.RF2].vel
msg.RightFinger3 = x.joint[ha.RF3].pos
msg.RightFinger3dot = x.joint[ha.RF3].vel
msg.RightFinger4 = x.joint[ha.RF4].pos
msg.RightFinger4dot = x.joint[ha.RF4].vel
msg.RightFinger5 = x.joint[ha.RF5].pos
msg.RightFinger5dot = x.joint[ha.RF5].vel
msg.TrunkYaw = x.joint[ha.WST].pos
msg.TrunkYawdot = x.joint[ha.WST].vel
msg.LeftHipYaw = x.joint[ha.LHY].pos
msg.LeftHipYawdot = x.joint[ha.LHY].vel
msg.LeftHipRoll = x.joint[ha.LHR].pos
msg.LeftHipRolldot = x.joint[ha.LHR].vel
msg.LeftHipPitch = x.joint[ha.LHP].pos
msg.LeftHipPitchdot = x.joint[ha.LHP].vel
msg.LeftKneePitch = x.joint[ha.LKN].pos
msg.LeftKneePitchdot = x.joint[ha.LKN].vel
msg.LeftAnklePitch = x.joint[ha.LAP].pos
msg.LeftAnklePitchdot = x.joint[ha.LAP].vel
msg.LeftAnkleRoll = x.joint[ha.LAR].pos
msg.LeftAnkleRolldot = x.joint[ha.LAR].vel
msg.RightHipYaw = x.joint[ha.RHY].pos
msg.RightHipYawdot = x.joint[ha.RHY].vel
msg.RightHipRoll = x.joint[ha.RHR].pos
msg.RightHipRolldot = x.joint[ha.RHR].vel
msg.RightHipPitch = x.joint[ha.RHP].pos
msg.RightHipPitchdot = x.joint[ha.RHP].vel
msg.RightKneePitch = x.joint[ha.RKN].pos
msg.RightKneePitchdot = x.joint[ha.RKN].vel
msg.RightAnklePitch = x.joint[ha.RAP].pos
msg.RightAnklePitchdot = x.joint[ha.RAP].vel
msg.RightAnkleRoll = x.joint[ha.RAR].pos
msg.RightAnkleRolldot = x.joint[ha.RAR].vel
return msg
