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)