def generate_rules(cls):
return [
ProductionRule(
child_type=PlaceSetting,
xyz_rule=SamePositionRule(
offset=torch.tensor([-0.33, 0., 0.8])),
rotation_rule=SameRotationRule(offset=torch.tensor(
RotationMatrix(RollPitchYaw(0., 0., 0.)).matrix()))),
ProductionRule(
child_type=PlaceSetting,
xyz_rule=SamePositionRule(
offset=torch.tensor([0.33, 0., 0.8])),
rotation_rule=SameRotationRule(offset=torch.tensor(
RotationMatrix(RollPitchYaw(0., 0., np.pi)).matrix()))),
ProductionRule(
child_type=PlaceSetting,
xyz_rule=SamePositionRule(
offset=torch.tensor([0., 0.33, 0.8])),
rotation_rule=SameRotationRule(offset=torch.tensor(
RotationMatrix(RollPitchYaw(0., 0., -np.pi /
2.)).matrix()))),
ProductionRule(child_type=PlaceSetting,
xyz_rule=SamePositionRule(
offset=torch.tensor([0., -0.33, 0.8])),
rotation_rule=SameRotationRule(offset=torch.tensor(
RotationMatrix(RollPitchYaw(0., 0., np.pi /
2.)).matrix())))
]
def generate_rules(cls):
return [
ProductionRule(
child_type=PlaceSetting,
xyz_rule=SamePositionRule(
offset=torch.tensor([-cls.DISTANCE_FROM_CENTER, 0., 0.])),
rotation_rule=SameRotationRule(offset=torch.tensor(
RotationMatrix(RollPitchYaw(0., 0., 0.)).matrix()))),
ProductionRule(
child_type=PlaceSetting,
xyz_rule=SamePositionRule(offset=torch.tensor(
[cls.DISTANCE_FROM_CENTER, 0., 0.]), ),
rotation_rule=SameRotationRule(offset=torch.tensor(
RotationMatrix(RollPitchYaw(0., 0., np.pi)).matrix()))),
ProductionRule(
child_type=PlaceSetting,
xyz_rule=SamePositionRule(
offset=torch.tensor([0., cls.DISTANCE_FROM_CENTER, 0.])),
rotation_rule=SameRotationRule(offset=torch.tensor(
RotationMatrix(RollPitchYaw(0., 0., -np.pi /
2.)).matrix()))),
ProductionRule(
child_type=PlaceSetting,
xyz_rule=SamePositionRule(
offset=torch.tensor([0., -cls.DISTANCE_FROM_CENTER, 0.])),
rotation_rule=SameRotationRule(offset=torch.tensor(
RotationMatrix(RollPitchYaw(0., 0., np.pi /
2.)).matrix())))
]
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 convert_scenes_yaml_to_observed_nodes(dataset_file,
type_map={},
model_map={}):
'''
Converts a scene list YAML file of the format described at
https://github.com/gizatt/drake_hydra_interact/blob/master/environments/README.md
into a list of nodes, using the dictionary type_map to map from metadata "type"
string to Node types.
'''
with open(dataset_file, "r") as f:
scenes_dict = yaml.load(f, Loader=Loader)
observed_node_sets = []
for scene_name, scene_info in scenes_dict.items():
observed_nodes = []
for object_info in scene_info["objects"]:
class_name = object_info["metadata"]["class"]
model_name = object_info["model_file"]
if model_name in model_map.keys():
this_type = model_map[model_name]
elif class_name in type_map.keys():
this_type = type_map[class_name]
else:
logging.warn(
"Environment had unknown model name / class name: %s, %s",
model_name, class_name)
continue
tf = RigidTransform(p=object_info["pose"]["xyz"],
rpy=RollPitchYaw(object_info["pose"]["rpy"]))
observed_nodes.append(this_type(drake_tf_to_torch_tf(tf)))
for object_info in scene_info["world_description"]["models"]:
if "metadata" in object_info.keys():
class_name = object_info["metadata"]["class"]
model_name = object_info["model_file"]
if model_name in model_map.keys():
this_type = model_map[model_name]
elif class_name in type_map.keys():
this_type = type_map[class_name]
else:
logging.warn(
"Environment had unknown model name / class name: %s, %s",
model_name, class_name)
continue
tf = RigidTransform(p=object_info["pose"]["xyz"],
rpy=RollPitchYaw(
object_info["pose"]["rpy"]))
observed_nodes.append(this_type(drake_tf_to_torch_tf(tf)))
observed_node_sets.append(observed_nodes)
return observed_node_sets
def CreateIiwaControllerPlant():
"""creates plant that includes only the robot and gripper, used for controllers."""
robot_sdf_path = FindResourceOrThrow(
"drake/manipulation/models/iiwa_description/iiwa7/iiwa7_no_collision.sdf")
gripper_sdf_path = FindResourceOrThrow(
"drake/manipulation/models/wsg_50_description/sdf/schunk_wsg_50_no_tip.sdf")
sim_timestep = 1e-3
plant_robot = MultibodyPlant(sim_timestep)
parser = Parser(plant=plant_robot)
parser.AddModelFromFile(robot_sdf_path)
parser.AddModelFromFile(gripper_sdf_path)
plant_robot.WeldFrames(
A=plant_robot.world_frame(),
B=plant_robot.GetFrameByName("iiwa_link_0"))
plant_robot.WeldFrames(
A=plant_robot.GetFrameByName("iiwa_link_7"),
B=plant_robot.GetFrameByName("body"),
X_AB=RigidTransform(RollPitchYaw(np.pi/2, 0, np.pi/2), np.array([0, 0, 0.114]))
)
plant_robot.mutable_gravity_field().set_gravity_vector([0, 0, 0])
plant_robot.Finalize()
link_frame_indices = []
for i in range(8):
link_frame_indices.append(
plant_robot.GetFrameByName("iiwa_link_" + str(i)).index())
return plant_robot, link_frame_indices
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 make_common_geometry_items(parent_item, tf, geometry):
# Parent item is "visual" or "collision" item
pose_item = et.SubElement(parent_item, "pose")
x, y, z = tf.translation()
r, p, y = RollPitchYaw(tf.rotation()).vector()
pose_item.text = "%f %f %f %f %f %f" % (x, y, z, r, p, y)
geometry_item = et.SubElement(parent_item, "geometry")
if isinstance(geometry, pydrake_geom.Box):
box_item = et.SubElement(geometry_item, "box")
size_item = et.SubElement(box_item, "size")
size_item.text = "%f %f %f" % (geometry.width(), geometry.depth(),
geometry.height())
elif isinstance(geometry, pydrake_geom.Sphere):
sphere_item = et.SubElement(geometry_item, "sphere")
radus_item = et.SubElement(sphere_item, "radus")
radius.text = "%f" % geometry_item.radius()
elif isinstance(geometry, pydrake_geom.Cylinder):
cylinder_item = et.SubElement(geometry_item, "cylinder")
radius_item = et.SubElement(cylinder_item, "radius")
radius_item.text = "%f" % geometry.radius()
length_item = et.SubElement(cylinder_item, "length")
length_item.text = "%f" % geometry.length()
elif isinstance(geometry, pydrake_geom.Mesh) or isinstance(
geometry, pydrake_geom.Convex):
mesh_item = et.SubElement(geometry_item, "mesh")
uri_item = et.SubElement(mesh_item, "uri")
uri_item.text = geometry.filename()
scale_item = et.SubElement(mesh_item, "scale")
scale_item.text = "%f %f %f" % (geometry.scale(), geometry.scale(),
geometry.scale())
if isinstance(geometry, pydrake_geom.Convex):
et.SubElement(mesh_item, '{drake.mit.edu}declare_convex')
else:
raise NotImplementedError("Geometry type ", geometry)
def __init__(self, with_knife=True):
self.table_top_z_in_world = 0.736 + 0.057 / 2
self.manipuland_body_indices = []
self.manipuland_params = []
self.tabletop_indices = []
self.guillotine_blade_index = None
self.with_knife = with_knife
self.model_index_dict = {}
r, p, y = 2.4, 1.9, 3.8
self.magic_rpy_offset = np.array([r, p, y])
self.magic_rpy_rotmat = RotationMatrix(RollPitchYaw(r, p, y)).matrix()
def generate_rules(cls):
return [
ProductionRule(
child_type=ChairDecoration,
xyz_rule=ParentFrameGaussianOffsetRule(
mean=torch.tensor([-0.4, 0.0, -cls.TABLE_HEIGHT]),
variance=torch.tensor([0.01, 0.01, 0.0001])),
rotation_rule=ParentFrameBinghamRotationRule.
from_rotation_and_rpy_variances(
RotationMatrix(RollPitchYaw(0., 0., -np.pi / 2.)),
np.array([1000, 1000, 100])))
]
def __init__(self, tf):
geom = PhysicsGeometryInfo()
rgba = np.random.uniform([0.85, 0.75, 0.45, 1.0],
[0.95, 0.85, 0.55, 1.0])
geom.register_geometry(
tf=drake_tf_to_torch_tf(
RigidTransform(p=[0., 0., .01 / 2.],
rpy=RollPitchYaw(0., np.pi / 2., 0.))),
geometry=pydrake_geom.Cylinder(radius=0.01, length=0.15),
color=rgba,
)
super().__init__(tf=tf, physics_geometry_info=geom, observed=True)
def generate_rules(cls):
return [
ProductionRule(child_type=Paper,
xyz_rule=WorldFramePlanarGaussianOffsetRule(
mean=torch.tensor([0.0, 0.0]),
variance=torch.tensor([0.05, 0.05]),
plane_transform=RigidTransform()),
rotation_rule=WorldFrameBinghamRotationRule.
from_rotation_and_rpy_variances(
RotationMatrix(RollPitchYaw(0., 0., 1.)),
[1000., 1000., 100.]))
]
def generate_rules(cls):
return [
ProductionRule(child_type=FirstChopstick,
xyz_rule=ParentFrameGaussianOffsetRule(
mean=torch.tensor([0.0, 0.0, 0.05]),
variance=torch.tensor([0.0005, 0.0005,
0.0001])),
rotation_rule=ParentFrameBinghamRotationRule.
from_rotation_and_rpy_variances(
RotationMatrix(RollPitchYaw(0., np.pi / 2.,
0.)),
np.array([1000, 1000, 1])))
]
def generate_rules(cls):
lb = torch.tensor([0.2, 0.2, 0.0])
ub = torch.tensor(
[cls.desk_size[0] - 0.2, cls.desk_size[1] - 0.2, 0.0])
rule = ProductionRule(child_type=ObjectCluster,
xyz_rule=WorldFramePlanarGaussianOffsetRule(
mean=torch.tensor([0.0, 0.0]),
variance=torch.tensor([0.2, 0.2]),
plane_transform=RigidTransform()),
rotation_rule=WorldFrameBinghamRotationRule.
from_rotation_and_rpy_variances(
RotationMatrix(RollPitchYaw(0., 0., 1.)),
[1000., 1000., 1.]))
return [rule]
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 test_se3_dist(set_seed):
# Should be zero distance
population_1 = drake_tf_to_torch_tf(
RigidTransform(p=np.zeros(3))).unsqueeze(0)
population_2 = drake_tf_to_torch_tf(
RigidTransform(p=np.zeros(3))).unsqueeze(0)
dists = se3_dist(population_1, population_2, beta=1., eps=0)
assert dists.shape == (1, 1) and torch.isclose(dists[0, 0],
torch.tensor(0.))
# Should be 1 distance
population_1 = drake_tf_to_torch_tf(
RigidTransform(p=np.zeros(3))).unsqueeze(0)
population_2 = drake_tf_to_torch_tf(
RigidTransform(p=np.array([0, 1, 0]))).unsqueeze(0)
dists = se3_dist(population_1, population_2, beta=1., eps=0)
assert dists.shape == (1, 1) and torch.isclose(dists[0, 0],
torch.tensor(1.))
# Should be pi distance
population_1 = drake_tf_to_torch_tf(
RigidTransform(p=np.zeros(3))).unsqueeze(0)
population_2 = drake_tf_to_torch_tf(
RigidTransform(p=np.zeros(3), rpy=RollPitchYaw(np.pi, 0.,
0.))).unsqueeze(0)
dists = se3_dist(population_1, population_2, beta=1., eps=0)
assert dists.shape == (1, 1) and torch.isclose(dists[0, 0],
torch.tensor(np.pi))
# Make sure it works at scale
M = 200
N = 100
population_1 = []
for k in range(M):
t = np.random.uniform(-1, 1, size=3)
R = UniformlyRandomRotationMatrix(set_seed)
tf = drake_tf_to_torch_tf(RigidTransform(p=t, R=R))
population_1.append(tf)
population_2 = []
for k in range(N):
t = np.random.uniform(-1, 1, size=3)
R = UniformlyRandomRotationMatrix(set_seed)
tf = drake_tf_to_torch_tf(RigidTransform(p=t, R=R))
population_2.append(tf)
population_1 = torch.stack(population_1)
population_2 = torch.stack(population_2)
dists = se3_dist(population_1, population_2, beta=1., eps=0)
assert dists.shape == (M, N) and torch.all(
torch.isfinite(dists)) and torch.all(dists >= 0)
def calc_x0(self, context, output):
x0 = np.zeros(6)
# Calc X_L_SP, the transform from the link to the setpoint
X_L_SP = RigidTransform() # For now, just make them the same
# Calc X_W_L
pose_L_rotational = self.GetInputPort("pose_L_rotational").Eval(
context)
pose_L_translational = self.GetInputPort("pose_L_translational").Eval(
context)
X_W_L = RigidTransform(p=pose_L_translational,
R=RotationMatrix(
RollPitchYaw(pose_L_rotational)))
# Calc X_W_SP
X_W_SP = X_W_L.multiply(X_L_SP)
translation = X_W_SP.translation()
rotation = RollPitchYaw(X_W_SP.rotation()).vector()
x0[:3] = rotation
x0[3:] = translation
output.SetFromVector(x0)
def CalcOutput(self, context, output):
q = self.GetInputPort("iiwa_pos_measured").Eval(context)
self.plant.SetPositions(self.plant_context, self.iiwa, q)
p_Ball = self.GetInputPort("ball_pose").Eval(context)
p_Ball_xy = p_Ball[:2]
v_Ball_xy = np.array(self.GetInputPort("ball_velocity").Eval(context))[:2]
R_Paddle = RollPitchYaw(self.plant.EvalBodyPoseInWorld(self.plant_context, self.P).rotation()).vector()
roll_current, pitch_current, yaw_current = R_Paddle[0], R_Paddle[1], R_Paddle[2]
# Prevent from going crazy when lost ball
# print(np.linalg.norm(p_Ball))
if np.linalg.norm(p_Ball) > 5:
output.SetFromVector([0, 0, 0])
return
k = np.array([.5, .5])
# roll_des = np.sign(B_y) * np.arctan(k * (1 - np.cos(B_y)))
# pitch_des = np.sign(B_x - centerpoint) * np.arctan(k * (1 - np.cos(B_x - centerpoint_x)))
sign_multiplier = np.array([1, 1])
if np.sign(p_Ball_xy[0] - CENTERPOINT) != np.sign(v_Ball_xy[0]):
# sign_multiplier[0] = .05
sign_multiplier[0] = 0.5 / (10 * abs(v_Ball_xy[0]) + 1)
if np.sign(p_Ball_xy[1]) != np.sign(v_Ball_xy[1]):
# sign_multiplier[1] = .05
sign_multiplier[1] = 0.5 / (10 * abs(v_Ball_xy[1]) + 1)
# deltas = sign_multiplier * np.sign(p_Ball_xy[:2] - CENTERPOINT) * np.arctan(k * (1 - np.cos(p_Ball_xy[:2] - CENTERPOINT)))
if p_Ball_xy[0] - CENTERPOINT == 0:
pitch_des = 0
else:
pitch_des = -sign_multiplier[0] * np.arctan(k[0] * (1 - np.cos(p_Ball_xy[0] - CENTERPOINT))/(p_Ball_xy[0] - CENTERPOINT))
if p_Ball[1] == 0:
roll_des = 0
else:
roll_des = sign_multiplier[1] * np.arctan(k[1] * (1 - np.cos(p_Ball_xy[1]))/(p_Ball_xy[1]))
yaw_des = 0
# print(f"Ball: [{B_x}, {B_y}, {X_B[2]}]\nRPY current: [{roll_current}, {pitch_current}, {yaw_current}]\nRPY desired: [{roll_des}, {pitch_des}, {yaw_des}]")
K_p = 5
dw = K_p*np.array([roll_des-roll_current, pitch_des-pitch_current, yaw_des - yaw_current])
# print(f"Commanded: {dw}\n")
# dw = np.zeros_like(dw)
output.SetFromVector(dw)
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 AddWsg(plant, iiwa_model_instance, roll=np.pi / 2.0, welded=False):
parser = Parser(plant)
if welded:
parser.package_map().Add(
"wsg_50_description",
os.path.dirname(
FindResourceOrThrow(
"drake/manipulation/models/wsg_50_description/package.xml"))
)
gripper = parser.AddModelFromFile(
FindResource("models/schunk_wsg_50_welded_fingers.sdf"), "gripper")
else:
gripper = parser.AddModelFromFile(
FindResourceOrThrow(
"drake/manipulation/models/"
"wsg_50_description/sdf/schunk_wsg_50_with_tip.sdf"))
X_7G = RigidTransform(RollPitchYaw(np.pi / 2.0, 0, roll), [0, 0, 0.114])
plant.WeldFrames(plant.GetFrameByName("iiwa_link_7", iiwa_model_instance),
plant.GetFrameByName("body", gripper), X_7G)
return gripper
# ubar[:,j]=test_Feedback_Linearization_controller_BS(state_out[:,j])
ubar[j,:]=u_values[j,:]
q_temp =x_values[j,3:7]
# print(np.dot(q_temp.T, q_temp))
quat_norm[j] =np.sqrt(np.dot(q_temp.T, q_temp))
quat_v_norm[j] = np.sqrt(np.dot(q_temp[1:4].T, q_temp[1:4]))
if q_temp[0]==1:
quat_v_norm[j] = 1
else:
quat_v_norm[j] = np.sqrt(np.dot(q_temp[1:4].T, q_temp[1:4]))
q_temp = q_temp/np.sqrt(np.dot(q_temp.T, q_temp))
quat_temp = Quaternion(q_temp) # Quaternion
R = RotationMatrix(quat_temp)
rpy[j,:]=RollPitchYaw(R).vector()
rpy_py[j,:]=quaternion_to_euler_angle(q_temp[0], q_temp[1], q_temp[2], q_temp[3])
u[j,:]=ubar[j,:]
# u[0,j]=x_values[7,j] # Control
u_max=np.max(u,0)
u_min=np.min(u,0)
u_max_max=np.max(u_max[1:4])
u_min_min=np.min(u_min[1:4])
if show_flag_q==1:
plt.clf()
fig = plt.figure(1).set_size_inches(6, 6)
for i in range(0,3):
# print("i:%d" %i)
plt.subplot(3, 1, i+1)
def do_point2point_icp_fit(scene_points, scene_points_normals,
model_points, model_points_normals,
params, vis=None):
n_attempts = params["n_attempts"]
max_iters_per_attempt = params["max_iters_per_attempt"]
model2scene = params["model2scene"]
outlier_rejection_ratio = params["outlier_rejection_ratio"]
outlier_max_distance = params["outlier_max_distance"]
if vis:
vis = vis["point2point_icp"]
scores = []
tfs = []
scales = []
tf_init = params["tf_init"]
if vis:
vis["fits"].delete()
if tf_init is not None and n_attempts != 1:
print "Setting n_attempts to 1, as you supplied"
print " an initial tf and this algorithm should be"
print " deterministic."
n_attempts = 1
for k in range(n_attempts):
# Init transform
if tf_init is None:
tf = np.eye(4)
tf[0:3, 3] = scene_points.mean(axis=1) + \
np.random.randn(3)*np.std(scene_points, axis=1)
tf[0:3, 0:3] = RotationMatrix(
RollPitchYaw(np.random.random(3)*2*np.pi)).matrix()
else:
tf = tf_init
scale = np.eye(4)
neigh = NearestNeighbors(n_neighbors=1)
if model2scene:
neigh.fit(scene_points.T)
else:
neigh.fit(model_points.T)
if vis:
obj_name = "fits/obj_%d" % k
draw_model_fit_pts(
vis[obj_name], model_points, tf, [1., 0.8, 0.])
num_steps_at_standstill = 0
for i in range(max_iters_per_attempt):
if vis:
vis[obj_name].set_transform(tf)
# Compute rest with new scaled model points
tf_model_points = transform_points(tf, model_points)
# (Re)compute nearest neighbors for some of these methods
if model2scene:
model_pts_inlier, scene_pts_inlier = \
get_inlier_model_and_scene_points_model2scene(
neigh, tf_model_points, scene_points,
outlier_max_distance, outlier_rejection_ratio)
else:
model_pts_inlier, scene_pts_inlier = \
get_inlier_model_and_scene_points_scene2model(
neigh, tf_model_points, scene_points, tf,
outlier_max_distance, outlier_rejection_ratio)
if vis:
# Vis correspondences
n_pts = scene_pts_inlier.shape[1]
pt = np.zeros(3, N)
tf = 4x4
lines = np.zeros([3, n_pts*2])
colors = np.zeros([4, n_pts*2]).T
inds = np.array(range(0, n_pts*2, 2))
lines[:, inds] = model_pts_inlier[0:3, :]
lines[:, inds+1] = scene_pts_inlier[0:3, :]
vis["corresp"].set_object(
meshcat.geometry.LineSegmentsGeometry(
lines, colors))
# Point-to-point ICP update with trimesh
new_tf, _, _ = trimesh.registration.procrustes(
model_pts_inlier[0:3, :].T,
scene_pts_inlier[0:3, :].T,
reflection=False,
translation=True,
scale=False)
tf = new_tf.dot(tf)
if np.allclose(np.diag(new_tf[0:3, 0:3]), [1., 1., 1.]):
angle_dist = 0.
else:
# Angle from rotation matrix
angle_dist = np.arccos(
(np.sum(np.diag(new_tf[0:3, 0:3])) - 1) / 2.)
euclid_dist = np.linalg.norm(new_tf[0:3, 3])
if euclid_dist < 0.0001 and angle_dist < 0.0001:
num_steps_at_standstill += 1
if num_steps_at_standstill > 10:
break
else:
num_steps_at_standstill = 0
# Compute final fit cost
tf_model_points = transform_points(tf, model_points)
if model2scene:
model_pts_inlier, scene_pts_inlier = \
get_inlier_model_and_scene_points_model2scene(
neigh, tf_model_points, scene_points,
outlier_max_distance, outlier_rejection_ratio)
else:
model_pts_inlier, scene_pts_inlier = \
get_inlier_model_and_scene_points_scene2model(
neigh, tf_model_points, scene_points, tf,
outlier_max_distance, outlier_rejection_ratio)
score = np.square((model_pts_inlier - scene_pts_inlier)).mean()
tfs.append(tf)
scores.append(score)
scales.append(scale)
if vis:
draw_model_fit_pts(
vis[obj_name], model_points, tf,
plt.cm.jet(score)[0:3])
# print "%d: Mean resulting surface SDF value: %f" % (k, score)
time.sleep(1.0)
best_run = np.argmin(scores)
# print "Best run was run %d with score %f" % (best_run, scores[best_run])
return tfs[best_run]
# 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):
# print("i:%d" %i)
plt.subplot(3, 1, i + 1)
# print("test:", num_state)
def CalcOutput(self, context, output):
# ============================ LOAD INPUTS ============================
# Positions and velocities
rot_vec_M = self.GetInputPort("pose_M_rotational").Eval(context)
p_M = self.GetInputPort("pose_M_translational").Eval(context)
omega_M = self.GetInputPort("vel_M_rotational").Eval(context)
v_M = self.GetInputPort("vel_M_translational").Eval(context)
# Manipulator inputs
M = np.array(self.GetInputPort("M").Eval(context)).reshape(
(self.nq, self.nq))
J = np.array(self.GetInputPort("J").Eval(context)).reshape(
(6, self.nq))
Jdot_qdot = np.expand_dims(
np.array(self.GetInputPort("Jdot_qdot").Eval(context)), 1)
Cv = np.expand_dims(np.array(self.GetInputPort("Cv").Eval(context)), 1)
# Impedance
K = np.diag(self.GetInputPort("K").Eval(context))
D = np.diag(self.GetInputPort("D").Eval(context))
x0 = np.expand_dims(self.GetInputPort("x0").Eval(context), 1)
dx0 = np.expand_dims(self.GetInputPort("dx0").Eval(context), 1)
# ==================== CALCULATE INTERMEDIATE TERMS ===================
# Actual values
d_x = np.expand_dims(np.array(list(omega_M) + list(v_M)), 1)
x = np.expand_dims(np.array(list(rot_vec_M) + list(p_M)), 1)
Mq = M
# TODO: Should this be pinv? (Copying from Sangbae's notes)
Mx = np.linalg.inv(np.matmul(np.matmul(J, np.linalg.inv(Mq)), J.T))
# ===================== CALCULATE CONTROL OUTPUTS =====================
# print(np.matmul(np.linalg.inv(Mq), Cv).shape)
Vq = Cv
compliance_terms = np.matmul(D, dx0 - d_x) \
+ \
np.matmul(K, x0 - x)
cancelation_terms = np.matmul(
Mx,
np.matmul(
np.matmul(J, np.linalg.inv(Mq)),
Vq
) \
- \
Jdot_qdot
)
F_ctrl = cancelation_terms + compliance_terms
tau_ctrl = np.matmul(J.T, F_ctrl)
# ======================== UPDATE DEBUG VALUES ========================
self.debug["dx0"].append(dx0)
self.debug["x0"].append(x0)
self.debug["times"].append(context.get_time())
# ======================== UPDATE VISUALIZATION =======================
visualization.AddMeshcatTriad(
self.meshcat,
"impedance_setpoint",
X_PT=RigidTransform(p=x0[3:].flatten(),
R=RotationMatrix(RollPitchYaw(x0[:3]))))
visualization.AddMeshcatTriad(
self.meshcat,
"manipulator_pose",
X_PT=RigidTransform(p=p_M.flatten(),
R=RotationMatrix(RollPitchYaw(rot_vec_M))))
output.SetFromVector(tau_ctrl.flatten())
def output_pose_M_rotational(self, context, output):
poses = self.GetInputPort("poses").Eval(context)
output.SetFromVector(
list(
RollPitchYaw(
poses[self.contact_body_idx].rotation()).vector()))
model_name="obj_ycb_%03d" % k)
obj_ids.append(k+2)
mbp.Finalize()
# Optional: set up a meshcat visualizer for the scene.
#meshcat = builder.AddSystem(
# MeshcatVisualizer(scene_graph, draw_period=0.0333))
#builder.Connect(scene_graph.get_pose_bundle_output_port(),
# meshcat.get_input_port(0))
# Figure out where we're putting the camera for the scene by
# pointing it inwards, and then applying a random yaw around
# the origin.
cam_quat_base = RollPitchYaw(
68.*np.pi/180.,
0.*np.pi/180,
38.6*np.pi/180.).ToQuaternion()
cam_trans_base = np.array([0.47, -0.54, 0.31])
cam_tf_base = RigidTransform(quaternion=cam_quat_base,
p=cam_trans_base)
# Rotate camera around origin
cam_additional_rotation = RigidTransform(
quaternion=RollPitchYaw(0., 0., np.random.uniform(0., np.pi*2.)).ToQuaternion(),
p=[0, 0, 0]
)
cam_tf_base = cam_additional_rotation.multiply(cam_tf_base)
cam_tfs = [cam_tf_base]
# Set up the blender color camera using that camera, and specifying
# the environment map and a material to apply to the "ground" object.
# If you wanted to further rotate the scene (to e.g. rotate the background
def generate_image(image_num):
filename_base = os.path.join(path, f"{image_num:05d}")
builder = pydrake.systems.framework.DiagramBuilder()
plant, scene_graph = pydrake.multibody.plant.AddMultibodyPlantSceneGraph(builder, time_step=0.0005)
parser = pydrake.multibody.parsing.Parser(plant)
parser.AddModelFromFile(pydrake.common.FindResourceOrThrow(
"drake/examples/manipulation_station/models/bin.sdf"))
plant.WeldFrames(plant.world_frame(), plant.GetFrameByName("bin_base"))
inspector = scene_graph.model_inspector()
instance_id_to_class_name = dict()
for object_num in range(rng.integers(1,10)):
this_object = ycb[rng.integers(len(ycb))]
class_name = os.path.splitext(this_object)[0]
sdf = pydrake.common.FindResourceOrThrow("drake/manipulation/models/ycb/sdf/" + this_object)
instance = parser.AddModelFromFile(sdf, f"object{object_num}")
frame_id = plant.GetBodyFrameIdOrThrow(
plant.GetBodyIndices(instance)[0])
geometry_ids = inspector.GetGeometries(
frame_id, pydrake.geometry.Role.kPerception)
for geom_id in geometry_ids:
instance_id_to_class_name[int(inspector.GetPerceptionProperties(geom_id).GetProperty("label", "id"))] = class_name
plant.Finalize()
if not debug:
with open(filename_base + ".json", "w") as f:
json.dump(instance_id_to_class_name, f)
renderer = "my_renderer"
scene_graph.AddRenderer(
renderer, pydrake.geometry.render.MakeRenderEngineVtk(pydrake.geometry.render.RenderEngineVtkParams()))
depth_camera = DepthRenderCamera(RenderCameraCore(
renderer, CameraInfo(width=640, height=480, fov_y=np.pi / 4.0),
ClippingRange(near=0.1, far=10.0),
RigidTransform()),
DepthRange(0.1, 10.0))
camera = builder.AddSystem(
pydrake.systems.sensors.RgbdSensor(parent_id=scene_graph.world_frame_id(),
X_PB=RigidTransform(
RollPitchYaw(np.pi, 0, np.pi/2.0),
[0, 0, .8]),
depth_camera=depth_camera,
show_window=False))
camera.set_name("rgbd_sensor")
builder.Connect(scene_graph.get_query_output_port(),
camera.query_object_input_port())
builder.ExportOutput(camera.color_image_output_port(), "color_image")
builder.ExportOutput(camera.label_image_output_port(), "label_image")
diagram = builder.Build()
while True:
simulator = pydrake.systems.analysis.Simulator(diagram)
context = simulator.get_mutable_context()
plant_context = plant.GetMyContextFromRoot(context)
z = 0.1
for body_index in plant.GetFloatingBaseBodies():
tf = RigidTransform(
pydrake.math.UniformlyRandomRotationMatrix(generator),
[rng.uniform(-.15,.15), rng.uniform(-.2, .2), z])
plant.SetFreeBodyPose(plant_context,
plant.get_body(body_index),
tf)
z += 0.1
try:
simulator.AdvanceTo(1.0)
break
except RuntimeError:
# I've chosen an aggressive simulation time step which works most
# of the time, but can fail occasionally.
pass
color_image = diagram.GetOutputPort("color_image").Eval(context)
label_image = diagram.GetOutputPort("label_image").Eval(context)
if debug:
plt.figure()
plt.subplot(121)
plt.imshow(color_image.data)
plt.axis('off')
plt.subplot(122)
plt.imshow(colorize_labels(label_image.data))
plt.axis('off')
plt.show()
else:
Image.fromarray(color_image.data).save(f"{filename_base}.png")
np.save(f"{filename_base}_mask", label_image.data)
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())
new_rbt = RigidBodyTree()
add_single_instance_to_rbt(new_rbt, config, instance_config,
instance_j)
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
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)
# print("quaternion_from_rotmax:", quat_from_rot.wxyz())
# Get E(q) matrix given unit quaternion q
E = np.zeros((3, 4))