-
Notifications
You must be signed in to change notification settings - Fork 16
/
WooferDynamics.py
128 lines (104 loc) · 4.27 KB
/
WooferDynamics.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
import numpy as np
import rotations
from WooferConfig import WOOFER_CONFIG
def FootSelector(binary_foot_selector):
return binary_foot_selector[[0,0,0,1,1,1,2,2,2,3,3,3]]
def CoordinateExpander(binary_coordinate_selector):
return binary_coordinate_selector[[0,1,2,0,1,2,0,1,2,0,1,2]]
def LegForwardKinematics(quat_orientation, joints):
"""
Gives the North-East-Down (NED)-style coordinates of the four feet. NED coordinates are coordinates in a noninertial reference frame
attached to the CoM of the robot. The axes of the NED frame are parallel to the x, y, and z axes in this simulation.
quat_orientation: unit quaternion for body orientation
joints: joint angles in qpos ordering
"""
def LegFK(abad, for_back, radial, handedness):
hands = {"left":1, "right":-1}
offset = hands[handedness]*(WOOFER_CONFIG.ABDUCTION_OFFSET)
leg_unrotated = np.array([0, offset, -WOOFER_CONFIG.LEG_L + radial])
R = rotations.euler2mat([abad, for_back, 0]) # rotation matrix for abduction, then forward back
foot = np.dot(R,leg_unrotated)
return foot
# Get foot locations in local body coordinates
# Right-handedness of frame means y is positive in the LEFT direction
foot_fr = LegFK(joints[0], joints[1], joints[2], "right") + np.array([WOOFER_CONFIG.LEG_FB, -WOOFER_CONFIG.LEG_LR, 0])
foot_fl = LegFK(joints[3], joints[4], joints[5], "left") + np.array([WOOFER_CONFIG.LEG_FB, WOOFER_CONFIG.LEG_LR, 0])
foot_br = LegFK(joints[6], joints[7], joints[8], "right") + np.array([-WOOFER_CONFIG.LEG_FB, -WOOFER_CONFIG.LEG_LR, 0])
foot_bl = LegFK(joints[9], joints[10],joints[11],"left") + np.array([-WOOFER_CONFIG.LEG_FB, WOOFER_CONFIG.LEG_LR, 0])
# Transform into world coordinates (centered around robot COM)
feet_col_stack = np.column_stack((foot_fr, foot_fl, foot_br, foot_bl))
feet_world = np.dot(rotations.quat2mat(quat_orientation), feet_col_stack)
return feet_world.T.reshape(12)
def FootLocationsWorld(state):
"""
state: dictionary of state variables
"""
feet_locs_floating_NED = LegForwardKinematics(state['q'], state['j'])
feet_locs_world = feet_locs_floating_NED + state['p'][[0,1,2,0,1,2,0,1,2,0,1,2]]
return feet_locs_world
def LegJacobian(beta, theta, r, abaduction_offset = 0):
"""
Gives the jacobian (in the body frame) of a leg given the joint values
beta : ab/aduction angle
theta : forward/back angle
r : radius of leg
return : Jacobian (J) where (x,y,z).T = J * (betad, thetad, rd).T
"""
i = np.array([1,0,0])
j = np.array([0,1,0])
k = np.array([0,0,1])
# leg pointing straight down
unrotated = np.array([0,abaduction_offset,-WOOFER_CONFIG.LEG_L + r])
# vector from leg hub to foot in body coordinates
p = np.dot(rotations.euler2mat([beta, theta, 0]), unrotated)
# directions of positive joint movement
theta_axis = np.dot(rotations.euler2mat([beta, 0, 0]), j)
beta_axis = i
radial_axis = k
dpdbeta = np.cross(beta_axis, p)
dpdtheta = np.cross(theta_axis, p)
dpdr = np.dot(rotations.euler2mat([beta,theta,0]), radial_axis)
return np.column_stack([dpdbeta, dpdtheta, dpdr])
def FootForceToJointTorques(F_world, leg_joints, body_quat, abaduction_offset = 0):
"""
Transforms a world-coordinate foot force into joint torques
leg_joints: 3-vector
"""
# Transform into body coordinates
F_body = np.matmul(rotations.quat2mat(body_quat).T, F_world)
# Transform into joint torques
(beta,theta,r) = tuple(leg_joints)
joint_torques = np.matmul(LegJacobian(beta, theta, r, abaduction_offset = abaduction_offset).T, F_body)
return joint_torques
def FeetContacts(_sim):
contacts = np.zeros(4)
for i in range(_sim.data.ncon):
contact = _sim.data.contact[i]
c1 = _sim.model.geom_id2name(contact.geom1)
c2 = _sim.model.geom_id2name(contact.geom2)
# print('contact', i)
# print('dist', contact.dist)
# print('geom1', contact.geom1, c1)
# print('geom2', contact.geom2, c2)
if c1 == 'floor':
if c2 == 'fr':
contacts[0] = 1
if c2 == 'fl':
contacts[1] = 1
if c2 == 'br':
contacts[2] = 1
if c2 == 'bl':
contacts[3] = 1
return contacts
def position(sim):
return sim.data.qpos[0:3]
def velocity(sim):
return sim.data.qvel[0:3]
def orientation(sim):
return sim.data.qpos[3:7]
def angular_velocity(sim):
return sim.data.qvel[3:6]
def joints(sim):
return sim.data.qpos[7:]
def joint_vel(sim):
return sim.data.qvel[6:]