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
new_rbt.compile()
single_object_rbts.append(new_rbt)
all_points = []
all_points_normals = []
all_points_labels = []
all_points_distances = [[] for i in range(len(config["instances"]))]
for i, viewpoint in enumerate(camera_config["viewpoints"]):
camera_tf = lookat(viewpoint["eye"], viewpoint["target"],
viewpoint["up"])
camera_tf = camera_tf.dot(transform_inverse(depth_camera_pose))
camera_rpy = RollPitchYaw(RotationMatrix(camera_tf[0:3, 0:3]))
q0 = np.zeros(6)
q0[0:3] = camera_tf[0:3, 3]
q0[3:6] = camera_rpy.vector()
depth_image_name = "%09d_depth.png" % (i)
depth_image = np.array(
imageio.imread(os.path.join(args.dir, depth_image_name))) / 1000.
depth_image_drake = Image[PixelType.kDepth32F](depth_image.shape[1],
depth_image.shape[0])
depth_image_drake.mutable_data[:, :, 0] = depth_image[:, :]
points = camera.ConvertDepthImageToPointCloud(
depth_image_drake, camera.depth_camera_info())
good_points_mask = np.all(np.isfinite(points), axis=0)
points = points[:, good_points_mask]
points = np.vstack([points, np.ones([1, points.shape[1]])])
# Transform them to camera base frame
points = np.dot(depth_camera_pose, points)
# and then to world frame
# Last body = camera floating base
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())
# print("q:",q[0])
v_norm = np.sqrt((np.dot(v, v))) # axis of rotation
v_normalized = v.T / v_norm
# print("vnorm:", v_norm)
# print("vnormalized", np.dot(v_normalized,v_normalized.T))
q[1:4, 0] = math.sin(theta / 2) * v.T / v_norm
print("q: ", q)
# print("q_norm: ", np.dot(q.T,q))
quat = Quaternion(q)
# unit quaternion to Rotation matrix
R = RotationMatrix(quat)
print("R:", R.matrix())
rpy = RollPitchYaw(R)
print("rpy", rpy.vector())
# print(R.matrix().dtype) # data type of Rotation matrix
# print(np.dot(R.matrix().T,R.matrix()))
# R\in SO(3) check
Iden = np.dot(R.matrix().T, R.matrix())
print("R^TR=:", Iden)
# get quaternion from rotationmatrix
quat_from_rot = RotationMatrix.ToQuaternion(R)
# print("quaternion_from_rotmax:", quat_from_rot.wxyz())
# Get E(q) matrix given unit quaternion q
E = np.zeros((3, 4))
E[0, :] = np.array([-1 * q[1], q[0], -1 * q[3], q[2]]).T