def convertLCM_Matlab(x):
msg = lcmt.lcmt_hubo2state()
msg.timestamp = time.time()
#Baselink state not included
msg.NKY = x.joint[ha.NKY].pos
msg.NKYdot = 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.LSP = x.joint[ha.LSP].pos
msg.LSPdot = x.joint[ha.LSP].vel
msg.LSR = x.joint[ha.LSR].pos
msg.LSRdot = x.joint[ha.LSR].vel
msg.LSY = x.joint[ha.LSY].pos
msg.LSYdot = x.joint[ha.LSY].vel
msg.LEB = x.joint[ha.LEB].pos
msg.LEBdot = x.joint[ha.LEB].vel
msg.LWY = x.joint[ha.LWY].pos
msg.LWYdot = x.joint[ha.LWY].vel
msg.LWP = x.joint[ha.LWP].pos
msg.LWPdot = x.joint[ha.LWP].vel
msg.RSP = x.joint[ha.RSP].pos
msg.RSPdot = x.joint[ha.RSP].vel
msg.RSR = x.joint[ha.RSR].pos
msg.RSRdot = x.joint[ha.RSR].vel
msg.RSY = x.joint[ha.RSY].pos
msg.RSYdot = x.joint[ha.RSY].vel
msg.REB = x.joint[ha.REB].pos
msg.REBdot = x.joint[ha.REB].vel
msg.RWY = x.joint[ha.RWY].pos
msg.RWYdot = x.joint[ha.RWY].vel
msg.RWP = x.joint[ha.RWP].pos
msg.RWPdot = x.joint[ha.RWP].vel
msg.leftThumbKnuckle3 = x.joint[ha.LF1].pos
msg.leftThumbKnuckle3dot = x.joint[ha.LF1].vel
msg.leftIndexKnuckle3 = x.joint[ha.LF2].pos
msg.leftIndexKnuckle3dot = x.joint[ha.LF2].vel
msg.leftMiddleKnuckle3 = x.joint[ha.LF3].pos
msg.leftMiddleKnuckle3dot = x.joint[ha.LF3].vel
msg.leftRingKnuckle3 = x.joint[ha.LF4].pos
msg.leftRingKnuckle3dot = x.joint[ha.LF4].vel
msg.leftPinkyKnuckle3 = x.joint[ha.LF5].pos
msg.leftPinkyKnuckle3dot = x.joint[ha.LF5].vel
msg.rightThumbKnuckle3 = x.joint[ha.RF1].pos
msg.rightThumbKnuckle3dot = x.joint[ha.RF1].vel
msg.rightIndexKnuckle3 = x.joint[ha.RF2].pos
msg.rightIndexKnuckle3dot = x.joint[ha.RF2].vel
msg.rightMiddleKnuckle3 = x.joint[ha.RF3].pos
msg.rightMiddleKnuckle3dot = x.joint[ha.RF3].vel
msg.rightRingKnuckle3 = x.joint[ha.RF4].pos
msg.rightRingKnuckle3dot = x.joint[ha.RF4].vel
msg.rightPinkyKnuckle3 = x.joint[ha.RF5].pos
msg.rightPinkyKnuckle3dot = x.joint[ha.RF5].vel
msg.WST = x.joint[ha.WST].pos
msg.WSTdot = x.joint[ha.WST].vel
msg.LHY = x.joint[ha.LHY].pos
msg.LHYdot = x.joint[ha.LHY].vel
msg.LHR = x.joint[ha.LHR].pos
msg.LHRdot = x.joint[ha.LHR].vel
msg.LHP = x.joint[ha.LHP].pos
msg.LHPdot = x.joint[ha.LHP].vel
msg.LKN = x.joint[ha.LKN].pos
msg.LKNdot = x.joint[ha.LKN].vel
msg.LAP = x.joint[ha.LAP].pos
msg.LAPdot = x.joint[ha.LAP].vel
msg.LAR = x.joint[ha.LAR].pos
msg.LARdot = x.joint[ha.LAR].vel
msg.RHY = x.joint[ha.RHY].pos
msg.RHYdot = x.joint[ha.RHY].vel
msg.RHR = x.joint[ha.RHR].pos
msg.RHRdot = x.joint[ha.RHR].vel
msg.RHP = x.joint[ha.RHP].pos
msg.RHPdot = x.joint[ha.RHP].vel
msg.RKN = x.joint[ha.RKN].pos
msg.RKNdot = x.joint[ha.RKN].vel
msg.RAP = x.joint[ha.RAP].pos
msg.RAPdot = x.joint[ha.RAP].vel
msg.RAR = x.joint[ha.RAR].pos
msg.RARdot = x.joint[ha.RAR].vel
return msg
def convertLCM_Matlab(x):
msg = lcmt.lcmt_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.LeftShoudlerYawdot = 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.RSRdot = x.joint[ha.RSR].vel
msg.RightShoulderYaw = x.joint[ha.RSY].pos
msg.RigthShoulderYawdot = 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.LeftHYdot = 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 = lcmt.lcmt_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.LeftShoudlerYawdot = 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.RSRdot = x.joint[ha.RSR].vel
msg.RightShoulderYaw = x.joint[ha.RSY].pos
msg.RigthShoulderYawdot = 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.LeftHYdot = 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
