def solve(self, orn , pos , bodytoFeet): bodytoFR4 = np.array([bodytoFeet[0,0],bodytoFeet[0,1],bodytoFeet[0,2]]) bodytoFL4 = np.array([bodytoFeet[1,0],bodytoFeet[1,1],bodytoFeet[1,2]]) bodytoBR4 = np.array([bodytoFeet[2,0],bodytoFeet[2,1],bodytoFeet[2,2]]) bodytoBL4 = np.array([bodytoFeet[3,0],bodytoFeet[3,1],bodytoFeet[3,2]]) "defines 4 vertices which rotates with the body" _bodytoFR0 = geo.transform(self.bodytoFR0 , orn, pos) _bodytoFL0 = geo.transform(self.bodytoFL0 , orn, pos) _bodytoBR0 = geo.transform(self.bodytoBR0 , orn, pos) _bodytoBL0 = geo.transform(self.bodytoBL0 , orn, pos) "defines coxa_frame to foot_frame leg vector neccesary for IK" FRcoord = bodytoFR4 - _bodytoFR0 FLcoord = bodytoFL4 - _bodytoFL0 BRcoord = bodytoBR4 - _bodytoBR0 BLcoord = bodytoBL4 - _bodytoBL0 "undo transformation of leg vector to keep feet still" undoOrn = -orn undoPos = -pos _FRcoord = geo.transform(FRcoord , undoOrn, undoPos) _FLcoord = geo.transform(FLcoord , undoOrn, undoPos) _BRcoord = geo.transform(BRcoord , undoOrn, undoPos) _BLcoord = geo.transform(BLcoord , undoOrn, undoPos) FR_angles = IK.solve_R(_FRcoord , self.coxa , self.femur , self.tibia) FL_angles = IK.solve_L(_FLcoord , self.coxa , self.femur , self.tibia) BR_angles = IK.solve_R(_BRcoord , self.coxa , self.femur , self.tibia) BL_angles = IK.solve_L(_BLcoord , self.coxa , self.femur , self.tibia) return FR_angles, FL_angles, BR_angles, BL_angles
def solve(self, bodytoFeet, robParm): pos = np.array([robParm.robotX, robParm.robotY, robParm.robotZ]) orn = np.array( [robParm.robotRoll, robParm.robotPitch, robParm.robotYaw]) bodytoFR4 = np.asarray( [bodytoFeet[0, 0], bodytoFeet[0, 1], bodytoFeet[0, 2]]) bodytoFL4 = np.asarray( [bodytoFeet[1, 0], bodytoFeet[1, 1], bodytoFeet[1, 2]]) bodytoBR4 = np.asarray( [bodytoFeet[2, 0], bodytoFeet[2, 1], bodytoFeet[2, 2]]) bodytoBL4 = np.asarray( [bodytoFeet[3, 0], bodytoFeet[3, 1], bodytoFeet[3, 2]]) # print(bodytoFR4) """defines 4 vertices which rotates with the body""" _bodytoFR0 = geo.transform(self.bodytoFR0, orn, pos) _bodytoFL0 = geo.transform(self.bodytoFL0, orn, pos) _bodytoBR0 = geo.transform(self.bodytoBR0, orn, pos) _bodytoBL0 = geo.transform(self.bodytoBL0, orn, pos) """defines coxa_frame to foot_frame leg vector neccesary for IK""" # print(_bodytoFR0) # print(orn) # print(pos) FRcoord = bodytoFR4 - _bodytoFR0 FLcoord = bodytoFL4 - _bodytoFL0 BRcoord = bodytoBR4 - _bodytoBR0 BLcoord = bodytoBL4 - _bodytoBL0 """undo transformation of leg vector to keep feet still""" undoOrn = -orn undoPos = -pos _FRcoord = geo.transform(FRcoord, undoOrn, undoPos) _FLcoord = geo.transform(FLcoord, undoOrn, undoPos) _BRcoord = geo.transform(BRcoord, undoOrn, undoPos) _BLcoord = geo.transform(BLcoord, undoOrn, undoPos) # _FRcoord[0] = 0.32901515505988754 # _FRcoord[1] = -0.0671 # _FRcoord[2] = -0.21125820159444264 FR_angles = IK.solve_R(_FRcoord, self.coxa, self.femur, self.tibia) FL_angles = IK.solve_L(_FLcoord, self.coxa, self.femur, self.tibia) BR_angles = IK.solve_R(_BRcoord, self.coxa, self.femur, self.tibia) BL_angles = IK.solve_L(_BLcoord, self.coxa, self.femur, self.tibia) _bodytofeetFR = _bodytoFR0 + _FRcoord _bodytofeetFL = _bodytoFL0 + _FLcoord _bodytofeetBR = _bodytoBR0 + _BRcoord _bodytofeetBL = _bodytoBL0 + _BLcoord _bodytofeet = np.matrix( [[_bodytofeetFR[0], _bodytofeetFR[1], _bodytofeetFR[2]], [_bodytofeetFL[0], _bodytofeetFL[1], _bodytofeetFL[2]], [_bodytofeetBR[0], _bodytofeetBR[1], _bodytofeetBR[2]], [_bodytofeetBL[0], _bodytofeetBL[1], _bodytofeetBL[2]]]) # print(FRcoord, _FRcoord) return FR_angles, FL_angles, BR_angles, BL_angles, _bodytofeet
orn_c[0] = orn_c[0]+err_att[0]*KP*0.005 orn_c[1] = orn_c[1]+err_att[1]*KP*0.005 orn_c[2] = orn[2] undoOrn_c = [-attitude_flt[0],-attitude_flt[1],-orn[2]*0] cnt1=cnt1+1 if cnt1>20: cnt1=0 print("attitude:",attitude_flt," orn: ",orn) if USE_ATT: orn=orn_c undoOrn=undoOrn_c "defines the 4 vertices which rotates with the body" #使用RT将机体跨关节转换到全局坐标 _bodytoFR0 = geo.transform(bodytoFR0 , orn, pos) _bodytoFL0 = geo.transform(bodytoFL0 , orn, pos) _bodytoBR0 = geo.transform(bodytoBR0 , orn, pos) _bodytoBL0 = geo.transform(bodytoBL0 , orn, pos) "defines coxa_frame to foot_frame leg vector neccesary for IK"#跨关节全局向量减去足端向量=全局坐标系下足到跨的向量 FRcoord = bodytoFR4 - _bodytoFR0 FLcoord = bodytoFL4 - _bodytoFL0 BRcoord = bodytoBR4 - _bodytoBR0 BLcoord = bodytoBL4 - _bodytoBL0 "undo transformation made on the body to keep foot still"#用RT将该向量转换到机体 _FRcoord = geo.transform(FRcoord , undoOrn, undoPos) _FLcoord = geo.transform(FLcoord , undoOrn, undoPos) _BRcoord = geo.transform(BRcoord , undoOrn, undoPos) _BLcoord = geo.transform(BLcoord , undoOrn, undoPos) "solves the angles needed to reach the desire foot position(in the leg_frame)"#计算在各腿下对应关节角度 FR_angles = IK.solve_R(_FRcoord)