def test_complementarity_constraints(self):
N = 50
self.planner.create_minimal_program(N, 1.0)
q_init = default_q()
q_init[6] = Atlas.PELVIS_HEIGHT # z position of pelvis
q_final = default_q()
q_final[0:4] = Quaternion(RollPitchYaw([0.0, 0.0, 0.0]).ToRotationMatrix().matrix()).wxyz()
q_final[4] = 0.5 # x position of pelvis
q_final[6] = Atlas.PELVIS_HEIGHT # z position of pelvis
self.planner.add_0th_order_constraints(q_init, q_final, True)
self.planner.add_1st_order_constraints()
self.planner.add_2nd_order_constraints()
self.planner.add_slack_constraints()
is_success, sol = self.planner.solve(self.planner.create_initial_guess())
if not is_success:
self.fail("Failed to find non-complementarity solution!")
print("Non-complementarity solution found!")
self.planner.add_eq8a_constraints()
self.planner.add_eq8b_constraints()
self.planner.add_eq8c_contact_force_constraints()
self.planner.add_eq8c_contact_distance_constraints()
self.planner.add_eq9a_constraints()
self.planner.add_eq9b_constraints()
is_success, sol = self.planner.solve(self.planner.create_guess(sol))
if not is_success:
self.fail("Failed to find complementarity solution!")
print("Complementarity solution found!")
self.planner.add_slack_cost()
is_success, sol = self.planner.solve(self.planner.create_guess(sol))
# self.planner.add_eq10_cost()
self.assertTrue(is_success)
visualize(sol.q)
pdb.set_trace()
def get_box_from_geom(scene_graph, visual_only=True):
# TODO: GeometryInstance, InternalGeometry, & GeometryContext to get the shape of objects
# TODO: get_contact_results_output_port
# https://github.com/RussTedrake/underactuated/blob/master/src/underactuated/meshcat_visualizer.py
# https://github.com/RobotLocomotion/drake/blob/master/lcmtypes/lcmt_viewer_draw.lcm
mock_lcm = DrakeMockLcm()
DispatchLoadMessage(scene_graph, mock_lcm)
load_robot_msg = lcmt_viewer_load_robot.decode(
mock_lcm.get_last_published_message("DRAKE_VIEWER_LOAD_ROBOT"))
# 'link', 'num_links'
#builder.Connect(scene_graph.get_pose_bundle_output_port(),
# viz.get_input_port(0))
# TODO: hash bodies instead
box_from_geom = {}
for body_index in range(load_robot_msg.num_links):
# 'geom', 'name', 'num_geom', 'robot_num'
link = load_robot_msg.link[body_index]
[source_name, frame_name] = link.name.split("::")
# source_name = 'Source_1'
model_index = link.robot_num
visual_index = 0
for geom in sorted(link.geom, key=lambda g: g.position[::-1]): # sort by z, y, x
# 'color', 'float_data', 'num_float_data', 'position', 'quaternion', 'string_data', 'type'
# string_data is empty...
if visual_only and (geom.color[3] == 0):
continue
# TODO: sort by lowest point on the bounding box?
# TODO: maybe just return the set of bodies in order and let the user decide what to with them
visual_index += 1 # TODO: affected by transparent visual
if geom.type == geom.BOX:
assert geom.num_float_data == 3
[width, length, height] = geom.float_data
extent = np.array([width, length, height]) / 2.
elif geom.type == geom.SPHERE:
assert geom.num_float_data == 1
[radius] = geom.float_data
extent = np.array([radius, radius, radius])
elif geom.type == geom.CYLINDER:
assert geom.num_float_data == 2
[radius, height] = geom.float_data
extent = np.array([radius, radius, height/2.])
#meshcat_geom = meshcat.geometry.Cylinder(
# geom.float_data[1], geom.float_data[0])
# In Drake, cylinders are along +z
#elif geom.type == geom.MESH:
# meshcat_geom = meshcat.geometry.ObjMeshGeometry.from_file(
# geom.string_data[0:-3] + "obj")
else:
#print("Robot {}, link {}, geometry {}: UNSUPPORTED GEOMETRY TYPE {} WAS IGNORED".format(
# model_index, frame_name, visual_index-1, geom.type))
continue
link_from_box = RigidTransform(
RotationMatrix(Quaternion(geom.quaternion)), geom.position).GetAsIsometry3() #.GetAsMatrix4()
box_from_geom[model_index, frame_name, visual_index-1] = \
(BoundingBox(np.zeros(3), extent), link_from_box, get_geom_name(geom))
return box_from_geom
def load(self):
"""
Loads `meshcat` visualization elements.
@pre The `scene_graph` used to construct this object must be part of a
fully constructed diagram (e.g. via `DiagramBuilder.Build()`).
"""
# Intercept load message via mock LCM.
mock_lcm = DrakeMockLcm()
DispatchLoadMessage(self._scene_graph, mock_lcm)
load_robot_msg = lcmt_viewer_load_robot.decode(
mock_lcm.get_last_published_message("DRAKE_VIEWER_LOAD_ROBOT"))
# Translate elements to `meshcat`.
for i in range(load_robot_msg.num_links):
link = load_robot_msg.link[i]
[source_name, frame_name] = link.name.split("::")
for j in range(link.num_geom):
geom = link.geom[j]
element_local_tf = RigidTransform(
RotationMatrix(Quaternion(geom.quaternion)),
geom.position).GetAsMatrix4()
if geom.type == geom.BOX:
assert geom.num_float_data == 3
meshcat_geom = meshcat.geometry.Box(geom.float_data)
elif geom.type == geom.SPHERE:
assert geom.num_float_data == 1
meshcat_geom = meshcat.geometry.Sphere(geom.float_data[0])
elif geom.type == geom.CYLINDER:
assert geom.num_float_data == 2
meshcat_geom = meshcat.geometry.Cylinder(
geom.float_data[1],
geom.float_data[0])
# In Drake, cylinders are along +z
# In meshcat, cylinders are along +y
# Rotate to fix this misalignment
extra_rotation = tf.rotation_matrix(
math.pi/2., [1, 0, 0])
element_local_tf[0:3, 0:3] = \
element_local_tf[0:3, 0:3].dot(
extra_rotation[0:3, 0:3])
elif geom.type == geom.MESH:
meshcat_geom = \
meshcat.geometry.ObjMeshGeometry.from_file(
geom.string_data[0:-3] + "obj")
else:
print "UNSUPPORTED GEOMETRY TYPE ", \
geom.type, " IGNORED"
continue
self.vis[self.prefix][source_name][str(link.robot_num)][
frame_name][str(j)]\
.set_object(meshcat_geom,
meshcat.geometry.MeshLambertMaterial(
color=Rgba2Hex(geom.color)))
self.vis[self.prefix][source_name][str(link.robot_num)][
frame_name][str(j)].set_transform(element_local_tf)
def CalcPositionOutput(self, context, output):
t = context.get_time()
X_G = RigidTransform(Quaternion(self.traj_R_G.value(t)),
self.traj_p_G.value(t))
self.plant.SetFreeBodyPose(self.plant_context, self.gripper_body, X_G)
wsg = self.traj_wsg_command.value(t)
self.left_finger_joint.set_translation(self.plant_context, -wsg / 2.0)
self.right_finger_joint.set_translation(self.plant_context, wsg / 2.0)
output.SetFromVector(self.plant.GetPositions(self.plant_context))
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 test_simple_trajectory(self):
plant = MultibodyPlant(0.001)
load_atlas(plant, add_ground=False)
T = 2.0
l_foot_pos_traj = PiecewisePolynomial.FirstOrderHold(
np.linspace(0, T, 5),
np.array([[0.00, 0.09, 0.00], [0.25, 0.09,
0.10], [0.50, 0.09, 0.00],
[0.50, 0.09, 0.00], [0.50, 0.09, 0.00]]).T)
r_foot_pos_traj = PiecewisePolynomial.FirstOrderHold(
np.linspace(0, T, 5),
np.array([[0.00, -0.09, 0.00], [0.00, -0.09, 0.00],
[0.00, -0.09, 0.00], [0.25, -0.09, 0.10],
[0.50, -0.09, 0.00]]).T)
pelvis_pos_traj = PiecewisePolynomial.FirstOrderHold(
[0.0, T],
np.array([[0.00, 0.00, Atlas.PELVIS_HEIGHT - 0.05],
[0.50, 0.00, Atlas.PELVIS_HEIGHT - 0.05]]).T)
null_orientation_traj = PiecewiseQuaternionSlerp([0.0, T], [
Quaternion([1.0, 0.0, 0.0, 0.0]),
Quaternion([1.0, 0.0, 0.0, 0.0])
])
generator = PoseGenerator(plant, [
Trajectory('l_foot', Atlas.FOOT_OFFSET, l_foot_pos_traj, 1e-3,
null_orientation_traj, 0.05),
Trajectory('r_foot', Atlas.FOOT_OFFSET, r_foot_pos_traj, 1e-3,
null_orientation_traj, 0.05),
Trajectory('pelvis', np.zeros(3), pelvis_pos_traj, 1e-2,
null_orientation_traj, 0.2)
])
for t in np.linspace(0, T, 50):
q = generator.get_ik(t)
if q is not None:
visualize(q)
else:
self.fail("Failed to find IK solution!")
time.sleep(0.2)
def test_0th_order(self):
N = 50
self.planner.create_minimal_program(N, 1)
q_init = default_q()
q_init[6] = 1.5 # z position of pelvis
q_final = default_q()
q_final[0:4] = Quaternion(RollPitchYaw([2*np.pi, np.pi, np.pi/2]).ToRotationMatrix().matrix()).wxyz()
q_final[4] = 1.0 # x position of pelvis
q_final[6] = 2.0 # z position of pelvis
q_final[getJointIndexInGeneralizedPositions(self.planner.plant_float, "r_leg_hpy")] = np.pi/6
self.planner.add_0th_order_constraints(q_init, q_final, False)
is_success, sol = self.planner.solve(self.planner.create_initial_guess())
self.assertTrue(is_success)
visualize(sol.q)
def get_box_from_geom(scene_graph, visual_only=True):
# https://github.com/RussTedrake/underactuated/blob/master/src/underactuated/meshcat_visualizer.py
# https://github.com/RobotLocomotion/drake/blob/master/lcmtypes/lcmt_viewer_draw.lcm
mock_lcm = DrakeMockLcm()
DispatchLoadMessage(scene_graph, mock_lcm)
load_robot_msg = lcmt_viewer_load_robot.decode(
mock_lcm.get_last_published_message("DRAKE_VIEWER_LOAD_ROBOT"))
box_from_geom = {}
for body_index in range(load_robot_msg.num_links):
# 'geom', 'name', 'num_geom', 'robot_num'
link = load_robot_msg.link[body_index]
[source_name, frame_name] = link.name.split("::")
model_index = link.robot_num
visual_index = 0
for geom in sorted(link.geom,
key=lambda g: g.position[::-1]): # sort by z, y, x
# 'color', 'float_data', 'num_float_data', 'position', 'quaternion', 'string_data', 'type'
if visual_only and (geom.color[3] == 0):
continue
visual_index += 1
if geom.type == geom.BOX:
assert geom.num_float_data == 3
[width, length, height] = geom.float_data
extent = np.array([width, length, height]) / 2.
elif geom.type == geom.SPHERE:
assert geom.num_float_data == 1
[radius] = geom.float_data
extent = np.array([radius, radius, radius])
elif geom.type == geom.CYLINDER:
assert geom.num_float_data == 2
[radius, height] = geom.float_data
extent = np.array([radius, radius, height / 2.])
# In Drake, cylinders are along +z
else:
continue
link_from_box = RigidTransform(
RotationMatrix(Quaternion(geom.quaternion)),
geom.position).GetAsIsometry3()
box_from_geom[model_index, frame_name, visual_index-1] = \
(BoundingBox(np.zeros(3), extent), link_from_box, get_geom_name(geom))
return box_from_geom
def test_contact_sequence_constraints(self):
N = 100
self.planner.create_minimal_program(N, 1.0)
q_init = default_q()
q_init[6] = Atlas.PELVIS_HEIGHT # z position of pelvis
q_final = default_q()
q_final[0:4] = Quaternion(RollPitchYaw([0.0, 0.0, 0.0]).ToRotationMatrix().matrix()).wxyz()
q_final[4] = 0.2 # x position of pelvis
q_final[6] = Atlas.PELVIS_HEIGHT # z position of pelvis
self.planner.add_0th_order_constraints(q_init, q_final, True)
self.planner.add_1st_order_constraints()
self.planner.add_2nd_order_constraints()
is_success, sol = self.planner.solve(self.planner.create_initial_guess(False))
if is_success:
self.planner.add_contact_sequence_constraints()
is_success, sol = self.planner.solve(self.planner.create_guess(sol, False))
visualize(sol.q)
pdb.set_trace()
self.assertTrue(is_success)
def test_2nd_order(self):
N = 50
self.planner.create_minimal_program(N, 1.0)
q_init = default_q()
q_init[6] = 1.0 # z position of pelvis
q_final = default_q()
q_final[0:4] = Quaternion(RollPitchYaw([0.0, 0.0, np.pi/6]).ToRotationMatrix().matrix()).wxyz()
q_final[4] = 1.0 # x position of pelvis
q_final[6] = 1.5 # z position of pelvis
q_final[getJointIndexInGeneralizedPositions(self.planner.plant_float, "r_leg_hpy")] = np.pi/6
self.planner.add_0th_order_constraints(q_init, q_final, False)
self.planner.add_1st_order_constraints()
self.planner.add_2nd_order_constraints()
is_success, sol = self.planner.solve(self.planner.create_initial_guess())
# if not is_success:
# self.fail("First order solution not found!")
# print("First order solution found!")
# is_success, sol = self.planner.solve(self.planner.create_guess(sol))
self.assertTrue(is_success)
visualize(sol.q)
pdb.set_trace()
def GetManipulatorDynamics(self, q, qd):
# Input argument
#- q = [x, y, z, q0, q1, q2, q3]\in \mathbb{R}^7
#- qd= [vx, vy, vz, wx, wy, wz ]\in \mathbb{R}^6
x = q[0] # (x,y,z) in inertial frame
y = q[1]
z = q[2]
q0 = q[3] # q0+q1i+q2j+qk
q1 = q[4]
q2 = q[5]
q3 = q[6]
xi1 = q[7]
vx = qd[0] # CoM velocity in inertial frame
vy = qd[1]
vz = qd[2]
wx = qd[3] # Body velocity in "body frame"
wy = qd[4]
wz = qd[5]
xi2 = qd[6]
xi30 = qd[7]
xi31 = qd[8]
xi32 = qd[9]
# Stack up the state q
x = np.array([q, qd])
# print("x:",x)
qv = np.array([q1, q2, q3])
r = np.array([x, y, z])
# print("qv",qv.shape)
v = np.array([vx, vy, vz])
quat_vec = np.vstack([q0, q1, q2, q3])
# print("quat_vec",quat_vec.shape)
w = np.array([wx, wy, wz])
q_norm = np.sqrt(np.dot(quat_vec.T, quat_vec))
q_normalized = quat_vec / q_norm
# print("q_norm: ",q_norm)
quat = Quaternion(q_normalized) # Quaternion
Rq = RotationMatrix(quat).matrix() # Rotational matrix
# print("\n#######")
# Translation from w to \dot{q}
Eq = np.zeros((3, 4))
w_hat = np.zeros((3, 4))
# Eq for body frame
Eq[0, :] = np.array([-1 * q1, q0, 1 * q3,
-1 * q2]) # [-1*q1, q0, -1*q3, q2]
Eq[1, :] = np.array([-1 * q2, -1 * q3, q0,
1 * q1]) # [-1*q2, q3, q0, -1*q1]
Eq[2, :] = np.array([-1 * q3, 1 * q2, -1 * q1,
q0]) # [-1*q3, -1*q2, q1, q0]
# Eq for world frame
# Eq[0,:] = np.array([-1*q1, q0, -1*q3, q2])
# Eq[1,:] = np.array([-1*q2, q3, q0, -1*q1])
# Eq[2,:] = np.array([-1*q3, -1*q2, q1, q0])
# quat_dot = np.dot(Eq.T,w)/2. # \dot{quat}=1/2*E(q)^Tw
# w x (Iw)
I = np.zeros((3, 3)) # Intertia matrix
Ixx = self.Ixx
Iyy = self.Iyy
Izz = self.Izz
I[0, 0] = Ixx
I[1, 1] = Iyy
I[2, 2] = Izz
I_inv = np.linalg.inv(I)
Iw = np.dot(I, w)
wIw = np.cross(w.T, Iw.T).T
# Aerodynamic drag and parameters
e1 = np.zeros(3)
e1[0] = 1
e3 = np.zeros(3)
e3[2] = 1
r_w = self.rw_l * e3
wr_w = np.cross(w.T, r_w) # w x r
fdw = -self.bw * (v + np.dot(Rq, wr_w))
# print("fdw: ", np.shape(v))
RqTfdw = np.dot(Rq.T, fdw)
taudw = np.cross(r_w, RqTfdw)
# print("fdw: ", fdw)
# print("w: ", w)
# print("Rq.Twr_w :", np.dot(Rq.T,fdw))
# print("Rqwr_w :", np.dot(Rq,fdw))
taudw_b = -self.bw * (np.dot(Rq.T, v) + np.cross(w, r_w))
taudw_b = np.cross(r_w, taudw_b)
# print("taudw - taudw_b", taudw-taudw_b)
vd = np.dot(
(Rq * q[7] - self.g * np.eye(3)), e3
) + fdw / self.m # \dot{v} = -ge3 +R(q)e3 u[0] : u[0] Thrust is a unit of acceleration
wd = -np.dot(I_inv, wIw) + np.dot(I_inv, qd[7:10]) + np.dot(
I_inv, taudw)
# kd = 1.5*1e-1
# wd = -np.dot(I_inv,wIw)+np.dot(I_inv,-kd*w) + np.dot(I_inv, taudw)
# print("taudw: ", np.dot(Rq.T,fdw.T))
w_hat = np.zeros((3, 3))
w_hat[0, :] = np.array([0, -w[2], w[1]])
w_hat[1, :] = np.array([w[2], 0, -w[0]])
w_hat[2, :] = np.array([-w[1], w[0], 0])
# wrw_byhat = np.dot(w_hat,r_w)
# print("wr_w - wrw_byhat", wr_w - wrw_byhat)
return (Rq, Eq, wIw, I_inv, fdw, taudw, vd, wd)
input_out = input_log.data()
# print("num_iteration,:", num_iteration)
# print("state_out dimension:,:", state_out.shape)
rpy = np.zeros((3, num_iteration)) # Convert to Euler Angle
ubar = np.zeros((4, num_iteration)) # Convert to Euler Angle
u = np.zeros((4, num_iteration)) # Convert to Euler Angle
for j in range(0, num_iteration):
# ubar[:,j]=test_Feedback_Linearization_controller_BS(state_out[:,j])
ubar[:, j] = input_out[:, j]
q_temp = state_out[3:7, j]
q_temp_norm = math.sqrt(np.dot(q_temp, q_temp))
q_temp = q_temp / q_temp_norm
quat_temp = Quaternion(q_temp) # Quaternion
R = RotationMatrix(quat_temp)
rpy[:, j] = RollPitchYaw(R).vector()
u[:, j] = ubar[:, j]
u[0, j] = state_out[7, j] # Control
# print(u)
# Visualize state and input traces
# print("times",state_log.data()[1,:])RollPitchYaw
if show_flag_q == 1:
plt.clf()
fig = plt.figure(1).set_size_inches(6, 6)
for i in range(0, 3):
def GetManipulatorDynamics(self, q, qd):
# Input argument
#- q = [x, y, z, q0, q1, q2, q3]\in \mathbb{R}^7
#- qd= [vx, vy, vz, wx, wy, wz ]\in \mathbb{R}^6
x = q[0] # (x,y,z) in inertial frame
y = q[1]
z = q[2]
q0 = q[3] # q0+q1i+q2j+qk
q1 = q[4]
q2 = q[5]
q3 = q[6]
xi1 = q[7]
vx = qd[0] # CoM velocity in inertial frame
vy = qd[1]
vz = qd[2]
wx = qd[3] # Body velocity in "body frame"
wy = qd[4]
wz = qd[5]
xi2 = qd[6]
# Stack up the state q
x = np.hstack([q, qd])
# print("x:",x)
qv = np.vstack([q1, q2, q3])
r = np.array([x, y, z])
# print("qv",qv.shape)
v = np.vstack([vx, vy, vz])
quat_vec = np.vstack([q0, q1, q2, q3])
# print("quat_vec",quat_vec.shape)
w = np.array([wx, wy, wz])
q_norm = np.sqrt(np.dot(quat_vec.T, quat_vec))
q_normalized = quat_vec / q_norm
# print("q_norm: ",q_norm)
quat = Quaternion(q_normalized) # Quaternion
Rq = RotationMatrix(quat).matrix() # Rotational matrix
# print("\n#######")
# Translation from w to \dot{q}
Eq = np.zeros((3, 4))
# Eq for body frame
Eq[0, :] = np.array([-1 * q1, q0, 1 * q3,
-1 * q2]) # [-1*q1, q0, -1*q3, q2]
Eq[1, :] = np.array([-1 * q2, -1 * q3, q0,
1 * q1]) # [-1*q2, q3, q0, -1*q1]
Eq[2, :] = np.array([-1 * q3, 1 * q2, -1 * q1,
q0]) # [-1*q3, -1*q2, q1, q0]
# Eq for world frame
# Eq[0,:] = np.array([-1*q1, q0, -1*q3, q2])
# Eq[1,:] = np.array([-1*q2, q3, q0, -1*q1])
# Eq[2,:] = np.array([-1*q3, -1*q2, q1, q0])
# quat_dot = np.dot(Eq.T,w)/2. # \dot{quat}=1/2*E(q)^Tw
# w x (Iw)
I = np.zeros((3, 3)) # Intertia matrix
Ixx = self.Ixx
Iyy = self.Iyy
Izz = self.Izz
I[0, 0] = Ixx
I[1, 1] = Iyy
I[2, 2] = Izz
I_inv = np.linalg.inv(I)
Iw = np.dot(I, w)
wIw = np.cross(w.T, Iw.T).T
return (Rq, Eq, wIw, I_inv)
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())
# extra yaw here.
offset_quat_base = RollPitchYaw(0., 0., 0.).ToQuaternion().wxyz()
os.system("mkdir -p /tmp/ycb_scene_%03d" % scene_k)
blender_color_cam = builder.AddSystem(BlenderColorCamera(
scene_graph,
draw_period=0.03333/2.,
camera_tfs=cam_tfs,
zmq_url="tcp://127.0.0.1:5556",
env_map_path="data/env_maps/aerodynamics_workshop_4k.hdr",
material_overrides=[
(".*ground.*",
{"material_type": "CC0_texture",
"path": "data/test_pbr_mats/Wood15/Wood15"}),
],
global_transform=RigidTransform(
p=[0, 0, 0], quaternion=Quaternion(offset_quat_base)
),
out_prefix="/tmp/ycb_scene_%03d/" % scene_k
))
builder.Connect(scene_graph.get_pose_bundle_output_port(),
blender_color_cam.get_input_port(0))
# Make a similar label camera to render label images for the scene.
blender_label_cam = builder.AddSystem(BlenderLabelCamera(
scene_graph,
draw_period=0.03333/2.,
camera_tfs=cam_tfs,
zmq_url="tcp://127.0.0.1:5557",
global_transform=RigidTransform(
p=[0, 0, 0], quaternion=Quaternion(offset_quat_base)
),
q = np.zeros((4, 1)) # quaternion setup
theta = math.pi / 2
# angle of rotation
v = np.array([0., 1., 0])
q[0] = math.cos(theta / 2) #q0
# 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)
from pydrake.all import PiecewiseQuaternionSlerp, Quaternion
import numpy as np
q1 = np.array([1.0, 0.0, 0.0, 0.0])
q2 = np.array([-0.5, -0.5, 0.5, 0.5])
slerp1 = PiecewiseQuaternionSlerp(breaks=[0, 1],
quaternions=[Quaternion(q1),
Quaternion(q2)])
for i in np.linspace(0, 1, 11):
print(f"1: {Quaternion(slerp1.value(i)).wxyz()}")
q3 = np.array([1.0, 0.0, 0.0, 0.0])
q4 = np.array([-0.5, -0.5, 0.4999999999999999, 0.5000000000000001])
slerp2 = PiecewiseQuaternionSlerp(
breaks=[0, 1], quaternions=[Quaternion(q3[0:4]),
Quaternion(q4[0:4])])
for i in np.linspace(0, 1, 11):
print(f"2: {Quaternion(slerp2.value(i)).wxyz()}")