def unpackLcmState(self, msgState):
utime = msgState.utime
rpy = RollPitchYaw(Quaternion(msgState.q[3:7]))
R_rpy = rpy.ToRotationMatrix().matrix()
q = np.concatenate((msgState.q[:3], rpy.vector(), msgState.q[7:]))
qd = np.concatenate(
(np.matmul(R_rpy,
msgState.v[3:6]), msgState.v[:3], msgState.v[6:]))
foot = [msgState.left_foot, msgState.right_foot]
return utime, q, qd, foot
def _DoCalcDiscreteVariableUpdates(self, context, events, discrete_state):
y = discrete_state.get_mutable_vector().get_mutable_value()
if self.sim:
x = self.EvalVectorInput(context, 0).CopyToVector()
q = x[0:self.nq]
qd = x[self.nq:]
utime = context.get_time() * 1000000
else:
utime = self.EvalVectorInput(context, 0).CopyToVector()
q = self.EvalVectorInput(context, 1).CopyToVector()
v = self.EvalVectorInput(context, 2).CopyToVector()
# foot_contact = self.EvalVectorInput(context, 3).CopyToVector()
# Convert from quaternions to Euler Angles
rpy = RollPitchYaw(Quaternion(q[3:7]))
R_rpy = rpy.ToRotationMatrix().matrix()
q = np.concatenate((q[:3], rpy.vector(), q[7:]))
qd = np.concatenate((np.matmul(R_rpy, v[3:6]), v[:3], v[6:]))
r = self.rtree
kinsol = r.doKinematics(q, qd)
if not self.initialized:
toe_l = r.transformPoints(kinsol, p_midfoot,
r.FindBodyIndex("toe_left"), 0)
toe_r = r.transformPoints(kinsol, p_midfoot,
r.FindBodyIndex("toe_right"), 0)
offset = np.array([1.28981386e-02, 2.02279085e-08])
yaw = q[5]
yaw_rot = np.array([[cos(yaw), -sin(yaw)], [sin(yaw), cos(yaw)]])
self.q_nom[:2] = (toe_l[:2, 0] + toe_r[:2, 0]) / 2 - np.matmul(
yaw_rot, offset)
self.q_nom[5] = yaw
print self.q_nom[:6]
self.initialized = True
# self.lcmgl.glColor3f(1, 0, 0)
# self.lcmgl.sphere(q[0], q[1], 0, 0.01, 20, 20)
# self.lcmgl.glColor3f(0, 0, 1)
# self.lcmgl.sphere(self.q_nom[0], self.q_nom[1], 0, 0.01, 20, 20)
# self.lcmgl.glColor3f(0, 1, 0)
# self.lcmgl.sphere((toe_l[0,0] + toe_r[0,0])/2, (toe_l[1,0] + toe_r[1,0])/2, 0, 0.01, 20, 20)
# self.lcmgl.switch_buffer()
# raise Exception()
H = r.massMatrix(kinsol)
C = r.dynamicsBiasTerm(kinsol, {}, None)
B = r.B
com = r.centerOfMass(kinsol)
Jcom = r.centerOfMassJacobian(kinsol)
Jcomdot_times_v = r.centerOfMassJacobianDotTimesV(kinsol)
phi = contactDistancesAndNormals(self.rtree, kinsol, self.lfoot,
self.rfoot)
[force_vec, JB] = contactJacobianBV(self.rtree, kinsol, self.lfoot,
self.rfoot, False)
# TODO: Look into contactConstraintsBV
Jp = contactJacobian(self.rtree, kinsol, self.lfoot, self.rfoot, False)
Jpdot_times_v = contactJacobianDotTimesV(self.rtree, kinsol,
self.lfoot, self.rfoot)
J4bar = r.positionConstraintsJacobian(kinsol, False)
J4bardot_times_v = r.positionConstraintsJacDotTimesV(kinsol)
#------------------------------------------------------------
# Problem Constraints ---------------------------------------
self.con_4bar.UpdateCoefficients(J4bar, -J4bardot_times_v)
if self.nc > 0:
dyn_con = np.zeros(
(self.nq, self.nq + self.nu + self.ncf + self.nf))
dyn_con[:, :self.nq] = H
dyn_con[:, self.nq:self.nq + self.nu] = -B
dyn_con[:,
self.nq + self.nu:self.nq + self.nu + self.ncf] = -J4bar.T
dyn_con[:, self.nq + self.nu + self.ncf:] = -JB
self.con_dyn.UpdateCoefficients(dyn_con, -C)
foot_con = np.zeros((self.neps, self.nq + self.neps))
foot_con[:, :self.nq] = Jp
foot_con[:, self.nq:] = -np.eye(self.neps)
self.con_foot.UpdateCoefficients(foot_con, -Jpdot_times_v)
else:
dyn_con = np.zeros((self.nq, self.nq + self.nu + self.ncf))
dyn_con[:, :self.nq] = H
dyn_con[:, self.nq:self.nq + self.nu] = -B
dyn_con[:, self.nq + self.nu:] = -J4bar.T
self.con_dyn.UpdateCoefficients(dyn_con, -C)
#------------------------------------------------------------
# Problem Costs ---------------------------------------------
com_ddot_des = self.Kp_com * (self.com_des -
com) - self.Kd_com * np.matmul(Jcom, qd)
qddot_des = np.matmul(self.Kp_qdd, self.q_nom - q) - np.matmul(
self.Kd_qdd, qd)
self.cost_qdd.UpdateCoefficients(
2 * np.diag(self.w_qdd),
-2 * np.matmul(qddot_des, np.diag(self.w_qdd)))
self.cost_u.UpdateCoefficients(
2 * np.diag(self.w_u),
-2 * np.matmul(self.u_des, np.diag(self.w_u)))
self.cost_slack.UpdateCoefficients(
2 * self.w_slack * np.eye(self.neps), np.zeros(self.neps))
result = self.solver.Solve(self.prog)
# print result
# print self.prog.GetSolverId().name()
y[:self.nu] = self.prog.GetSolution(self.u)
# np.set_printoptions(precision=4, suppress=True, linewidth=1000)
# print qddot_des
# print self.prog.GetSolution(self.qddot)
# print self.prog.GetSolution(self.u)
# print self.prog.GetSolution(self.beta)
# print self.prog.GetSolution(self.bar)
# print self.prog.GetSolution(self.eps)
# print
# return
if not self.sim:
y[8 * self.nu] = utime
y[self.nu:8 * self.nu] = np.zeros(7 * self.nu)
return
if (self.sim and self.lcmgl is not None):
grf = np.zeros((3, self.nc))
for ii in range(4):
grf[:, ii] = np.matmul(
force_vec[:, 4 * ii:4 * ii + 4],
self.prog.GetSolution(self.beta)[4 * ii:4 * ii + 4])
pts = forwardKinTerrainPoints(self.rtree, kinsol, self.lfoot,
self.rfoot)
cop_x = np.matmul(pts[0], grf[2]) / np.sum(grf[2])
cop_y = np.matmul(pts[1], grf[2]) / np.sum(grf[2])
m = H[0, 0]
r_ddot = self.prog.GetSolution(self.qddot)[:3]
cop_x2 = q[0] - (q[2] * r_ddot[0]) / (r_ddot[2] + 9.81)
cop_y2 = q[1] - (q[2] * r_ddot[1]) / (r_ddot[2] + 9.81)
qdd = (qd[:3] - self.last_v) / 0.0005
cop_x3 = q[0] - (q[2] * qdd[0]) / (qdd[2] + 9.81)
cop_y3 = q[1] - (q[2] * qdd[1]) / (qdd[2] + 9.81)
self.last_v = qd[:3]
# Draw COM
self.lcmgl.glColor3f(0, 0, 1)
self.lcmgl.sphere(com[0], com[1], 0, 0.01, 20, 20)
# Draw COP
self.lcmgl.glColor3f(0, 1, 0)
self.lcmgl.sphere(cop_x, cop_y, 0, 0.01, 20, 20)
self.lcmgl.glColor3f(0, 1, 1)
self.lcmgl.sphere(cop_x2, cop_y2, 0, 0.01, 20, 20)
self.lcmgl.glColor3f(1, 0, 0)
self.lcmgl.sphere(cop_x3, cop_y3, 0, 0.01, 20, 20)
# Draw support polygon
self.lcmgl.glColor4f(1, 0, 0, 0.5)
self.lcmgl.glLineWidth(3)
self.lcmgl.glBegin(0x0001)
self.lcmgl.glVertex3d(pts[0, 0], pts[1, 0], pts[2, 0])
self.lcmgl.glVertex3d(pts[0, 1], pts[1, 1], pts[2, 1])
self.lcmgl.glVertex3d(pts[0, 1], pts[1, 1], pts[2, 1])
self.lcmgl.glVertex3d(pts[0, 3], pts[1, 3], pts[2, 3])
self.lcmgl.glVertex3d(pts[0, 3], pts[1, 3], pts[2, 3])
self.lcmgl.glVertex3d(pts[0, 2], pts[1, 2], pts[2, 2])
self.lcmgl.glVertex3d(pts[0, 2], pts[1, 2], pts[2, 2])
self.lcmgl.glVertex3d(pts[0, 0], pts[1, 0], pts[2, 0])
self.lcmgl.glEnd()
self.lcmgl.switch_buffer()
# Print the current time, if requested,
# as an indicator of how far simulation has
# progressed.
if (self.sim and self.print_period
and context.get_time() - self.last_print_time >=
self.print_period):
print "t: ", context.get_time()
self.last_print_time = context.get_time()
# print qddot_des
for ii in range(4):
print force_vec[:, 4 * ii:4 * ii + 4].dot(
self.prog.GetSolution(self.beta)[4 * ii:4 * ii + 4])
if (self.sim and self.cost_pub):
msgCost = self.packLcmCost(utime,
self.prog.GetSolution(self.qddot),
qddot_des,
self.prog.GetSolution(self.u),
self.prog.GetSolution(self.eps))
lc.publish("CASSIE_COSTS", msgCost.encode())
empty = np.zeros(self.nu)
msgCommand = self.packLcmCommand(utime,
self.prog.GetSolution(self.u),
empty, empty, empty, empty, empty,
empty, empty)
lc.publish("CASSIE_COMMAND", msgCommand.encode())