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