def test_AddAngleBetweenVectorsConstraint(self):
na_A = np.array([0.2, -0.4, 0.9])
nb_B = np.array([1.4, -0.1, 1.8])
angle_lower = 0.2 * math.pi
angle_upper = 0.2 * math.pi
self.ik_two_bodies.AddAngleBetweenVectorsConstraint(
frameA=self.body1_frame, na_A=na_A,
frameB=self.body2_frame, nb_B=nb_B,
angle_lower=angle_lower, angle_upper=angle_upper)
result = mp.Solve(self.prog)
self.assertTrue(result.is_success())
q_val = result.GetSolution(self.q)
body1_quat = self._body1_quat(q_val)
body2_quat = self._body2_quat(q_val)
body1_rotmat = Quaternion(body1_quat).rotation()
body2_rotmat = Quaternion(body2_quat).rotation()
na_W = body1_rotmat.dot(na_A)
nb_W = body2_rotmat.dot(nb_B)
angle = math.acos(na_W.transpose().dot(nb_W) /
(np.linalg.norm(na_W) * np.linalg.norm(nb_W)))
self.assertLess(math.fabs(angle - angle_lower), 1E-6)
with warnings.catch_warnings(record=True) as w:
warnings.simplefilter('always', DrakeDeprecationWarning)
self.assertEqual(
self.prog.Solve(), mp.SolutionResult.kSolutionFound)
self.assertTrue(np.allclose(
self.prog.GetSolution(self.q), q_val))
self.assertEqual(len(w), 2)
def test_rotation_matrix(self):
# - Constructors.
R = mut.RotationMatrix()
self.assertTrue(
np.allclose(mut.RotationMatrix(other=R).matrix(), np.eye(3)))
self.assertTrue(np.allclose(R.matrix(), np.eye(3)))
self.assertTrue(np.allclose(copy.copy(R).matrix(), np.eye(3)))
self.assertTrue(
np.allclose(mut.RotationMatrix.Identity().matrix(), np.eye(3)))
R = mut.RotationMatrix(R=np.eye(3))
self.assertTrue(np.allclose(R.matrix(), np.eye(3)))
R = mut.RotationMatrix(quaternion=Quaternion.Identity())
self.assertTrue(np.allclose(R.matrix(), np.eye(3)))
R = mut.RotationMatrix(rpy=mut.RollPitchYaw(rpy=[0, 0, 0]))
self.assertTrue(np.allclose(R.matrix(), np.eye(3)))
# - Nontrivial quaternion.
q = Quaternion(wxyz=[0.5, 0.5, 0.5, 0.5])
R = mut.RotationMatrix(quaternion=q)
q_R = R.ToQuaternion()
self.assertTrue(np.allclose(q.wxyz(), q_R.wxyz()))
# - Inverse.
R_I = R.inverse().multiply(R)
self.assertTrue(np.allclose(R_I.matrix(), np.eye(3)))
if six.PY3:
self.assertTrue(
np.allclose(eval("R.inverse() @ R").matrix(), np.eye(3)))
def test_AddAngleBetweenVectorsConstraint(self):
na_A = np.array([0.2, -0.4, 0.9])
nb_B = np.array([1.4, -0.1, 1.8])
angle_lower = 0.2 * math.pi
angle_upper = 0.2 * math.pi
self.ik_two_bodies.AddAngleBetweenVectorsConstraint(
frameA=self.body1_frame, na_A=na_A,
frameB=self.body2_frame, nb_B=nb_B,
angle_lower=angle_lower, angle_upper=angle_upper)
result = self.prog.Solve()
self.assertEqual(result, mp.SolutionResult.kSolutionFound)
q_val = self.prog.GetSolution(self.q)
body1_quat = self._body1_quat(q_val)
body2_quat = self._body2_quat(q_val)
body1_rotmat = Quaternion(body1_quat).rotation()
body2_rotmat = Quaternion(body2_quat).rotation()
na_W = body1_rotmat.dot(na_A)
nb_W = body2_rotmat.dot(nb_B)
angle = math.acos(na_W.transpose().dot(nb_W) /
(np.linalg.norm(na_W) * np.linalg.norm(nb_W)))
self.assertLess(math.fabs(angle - angle_lower), 1E-6)
def test_pose_vector(self):
value = PoseVector()
self.assertTrue(isinstance(value, BasicVector))
self.assertTrue(isinstance(copy.copy(value), PoseVector))
self.assertTrue(isinstance(value.Clone(), PoseVector))
self.assertEqual(value.size(), PoseVector.kSize)
# - Accessors.
self.assertTrue(isinstance(value.get_isometry(), Isometry3))
self.assertTrue(isinstance(value.get_rotation(), Quaternion))
self.assertTrue(isinstance(value.get_translation(), np.ndarray))
# - Value.
self.assertTrue(
np.allclose(value.get_isometry().matrix(), np.eye(4, 4)))
# - Mutators.
p = [0, 1, 2]
q = Quaternion(wxyz=normalized([0.1, 0.3, 0.7, 0.9]))
X_expected = Isometry3(q, p)
value.set_translation(p)
value.set_rotation(q)
self.assertTrue(
np.allclose(value.get_isometry().matrix(), X_expected.matrix()))
# - Ensure ordering is ((px, py, pz), (qw, qx, qy, qz))
vector_expected = np.hstack((p, q.wxyz()))
vector_actual = value.get_value()
self.assertTrue(np.allclose(vector_actual, vector_expected))
# - Fully-parameterized constructor.
value1 = PoseVector(rotation=q, translation=p)
self.assertTrue(
np.allclose(value1.get_isometry().matrix(), X_expected.matrix()))
def test_AddPositionConstraint(self):
p_BQ = np.array([0.2, 0.3, 0.5])
p_AQ_lower = np.array([-0.1, -0.2, -0.3])
p_AQ_upper = np.array([-0.05, -0.12, -0.28])
self.ik_two_bodies.AddPositionConstraint(
frameB=self.body1_frame, p_BQ=p_BQ,
frameA=self.body2_frame,
p_AQ_lower=p_AQ_lower, p_AQ_upper=p_AQ_upper)
result = mp.Solve(self.prog)
self.assertTrue(result.is_success())
q_val = result.GetSolution(self.q)
body1_quat = self._body1_quat(q_val)
body1_pos = self._body1_xyz(q_val)
body2_quat = self._body2_quat(q_val)
body2_pos = self._body2_xyz(q_val)
body1_rotmat = Quaternion(body1_quat).rotation()
body2_rotmat = Quaternion(body2_quat).rotation()
p_AQ = body2_rotmat.transpose().dot(
body1_rotmat.dot(p_BQ) + body1_pos - body2_pos)
self.assertTrue(np.greater(p_AQ, p_AQ_lower -
1E-6 * np.ones((3, 1))).all())
self.assertTrue(np.less(p_AQ, p_AQ_upper +
1E-6 * np.ones((3, 1))).all())
with warnings.catch_warnings(record=True) as w:
warnings.simplefilter('always', DrakeDeprecationWarning)
self.assertEqual(
self.prog.Solve(), mp.SolutionResult.kSolutionFound)
self.assertTrue(np.allclose(
self.prog.GetSolution(self.q), q_val))
self.assertEqual(len(w), 2)
def test_AddPositionConstraint(self):
p_BQ = np.array([0.2, 0.3, 0.5])
p_AQ_lower = np.array([-0.1, -0.2, -0.3])
p_AQ_upper = np.array([-0.05, -0.12, -0.28])
self.ik_two_bodies.AddPositionConstraint(
frameB=self.body1_frame, p_BQ=p_BQ,
frameA=self.body2_frame,
p_AQ_lower=p_AQ_lower, p_AQ_upper=p_AQ_upper)
result = self.prog.Solve()
self.assertEqual(result, mp.SolutionResult.kSolutionFound)
q_val = self.prog.GetSolution(self.q)
body1_quat = self._body1_quat(q_val)
body1_pos = self._body1_xyz(q_val)
body2_quat = self._body2_quat(q_val)
body2_pos = self._body2_xyz(q_val)
body1_rotmat = Quaternion(body1_quat).rotation()
body2_rotmat = Quaternion(body2_quat).rotation()
p_AQ = body2_rotmat.transpose().dot(
body1_rotmat.dot(p_BQ) + body1_pos - body2_pos)
self.assertTrue(np.greater(p_AQ, p_AQ_lower -
1E-6 * np.ones((3, 1))).all())
self.assertTrue(np.less(p_AQ, p_AQ_upper +
1E-6 * np.ones((3, 1))).all())
def test_AddGazeTargetConstraint(self):
p_AS = np.array([0.1, 0.2, 0.3])
n_A = np.array([0.3, 0.5, 1.2])
p_BT = np.array([1.1, 0.2, 1.5])
cone_half_angle = 0.2 * math.pi
self.ik_two_bodies.AddGazeTargetConstraint(
frameA=self.body1_frame, p_AS=p_AS, n_A=n_A,
frameB=self.body2_frame, p_BT=p_BT,
cone_half_angle=cone_half_angle)
result = self.prog.Solve()
self.assertEqual(result, mp.SolutionResult.kSolutionFound)
q_val = self.prog.GetSolution(self.q)
body1_quat = self._body1_quat(q_val)
body1_pos = self._body1_xyz(q_val)
body2_quat = self._body2_quat(q_val)
body2_pos = self._body2_xyz(q_val)
body1_rotmat = Quaternion(body1_quat).rotation()
body2_rotmat = Quaternion(body2_quat).rotation()
p_WS = body1_pos + body1_rotmat.dot(p_AS)
p_WT = body2_pos + body2_rotmat.dot(p_BT)
p_ST_W = p_WT - p_WS
n_W = body1_rotmat.dot(n_A)
self.assertGreater(p_ST_W.dot(n_W), np.linalg.norm(
p_ST_W) * np.linalg.norm(n_W) * math.cos(cone_half_angle) - 1E-6)
def test_rotation_matrix(self):
# - Constructors.
R = mut.RotationMatrix()
self.assertTrue(np.allclose(
mut.RotationMatrix(other=R).matrix(), np.eye(3)))
self.assertTrue(np.allclose(R.matrix(), np.eye(3)))
self.assertTrue(np.allclose(copy.copy(R).matrix(), np.eye(3)))
self.assertTrue(np.allclose(
mut.RotationMatrix.Identity().matrix(), np.eye(3)))
R = mut.RotationMatrix(R=np.eye(3))
self.assertTrue(np.allclose(R.matrix(), np.eye(3)))
R = mut.RotationMatrix(quaternion=Quaternion.Identity())
self.assertTrue(np.allclose(R.matrix(), np.eye(3)))
R = mut.RotationMatrix(rpy=mut.RollPitchYaw(rpy=[0, 0, 0]))
self.assertTrue(np.allclose(R.matrix(), np.eye(3)))
# - Nontrivial quaternion.
q = Quaternion(wxyz=[0.5, 0.5, 0.5, 0.5])
R = mut.RotationMatrix(quaternion=q)
q_R = R.ToQuaternion()
self.assertTrue(np.allclose(q.wxyz(), q_R.wxyz()))
# - Inverse.
R_I = R.inverse().multiply(R)
self.assertTrue(np.allclose(R_I.matrix(), np.eye(3)))
if six.PY3:
self.assertTrue(np.allclose(
eval("R.inverse() @ R").matrix(), np.eye(3)))
def test_pose_vector(self):
value = PoseVector()
self.assertTrue(isinstance(value, BasicVector))
self.assertTrue(isinstance(copy.copy(value), PoseVector))
self.assertTrue(isinstance(value.Clone(), PoseVector))
self.assertEqual(value.size(), PoseVector.kSize)
# - Accessors.
self.assertTrue(isinstance(
value.get_isometry(), Isometry3))
self.assertTrue(isinstance(
value.get_rotation(), Quaternion))
self.assertTrue(isinstance(
value.get_translation(), np.ndarray))
# - Value.
self.assertTrue(np.allclose(
value.get_isometry().matrix(), np.eye(4, 4)))
# - Mutators.
p = [0, 1, 2]
q = Quaternion(wxyz=normalized([0.1, 0.3, 0.7, 0.9]))
X_expected = Isometry3(q, p)
value.set_translation(p)
value.set_rotation(q)
self.assertTrue(np.allclose(
value.get_isometry().matrix(), X_expected.matrix()))
# - Ensure ordering is ((px, py, pz), (qw, qx, qy, qz))
vector_expected = np.hstack((p, q.wxyz()))
vector_actual = value.get_value()
self.assertTrue(np.allclose(vector_actual, vector_expected))
# - Fully-parameterized constructor.
value1 = PoseVector(rotation=q, translation=p)
self.assertTrue(np.allclose(
value1.get_isometry().matrix(), X_expected.matrix()))
def test_AddGazeTargetConstraint(self):
p_AS = np.array([0.1, 0.2, 0.3])
n_A = np.array([0.3, 0.5, 1.2])
p_BT = np.array([1.1, 0.2, 1.5])
cone_half_angle = 0.2 * math.pi
self.ik_two_bodies.AddGazeTargetConstraint(
frameA=self.body1_frame, p_AS=p_AS, n_A=n_A,
frameB=self.body2_frame, p_BT=p_BT,
cone_half_angle=cone_half_angle)
result = mp.Solve(self.prog)
self.assertTrue(result.is_success())
q_val = result.GetSolution(self.q)
body1_quat = self._body1_quat(q_val)
body1_pos = self._body1_xyz(q_val)
body2_quat = self._body2_quat(q_val)
body2_pos = self._body2_xyz(q_val)
body1_rotmat = Quaternion(body1_quat).rotation()
body2_rotmat = Quaternion(body2_quat).rotation()
p_WS = body1_pos + body1_rotmat.dot(p_AS)
p_WT = body2_pos + body2_rotmat.dot(p_BT)
p_ST_W = p_WT - p_WS
n_W = body1_rotmat.dot(n_A)
self.assertGreater(p_ST_W.dot(n_W), np.linalg.norm(
p_ST_W) * np.linalg.norm(n_W) * math.cos(cone_half_angle) - 1E-6)
with warnings.catch_warnings(record=True) as w:
warnings.simplefilter('always', DrakeDeprecationWarning)
self.assertEqual(
self.prog.Solve(), mp.SolutionResult.kSolutionFound)
q_val = self.prog.GetSolution(self.q)
body1_quat = self._body1_quat(q_val)
body1_pos = self._body1_xyz(q_val)
body2_quat = self._body2_quat(q_val)
body2_pos = self._body2_xyz(q_val)
body1_rotmat = Quaternion(body1_quat).rotation()
body2_rotmat = Quaternion(body2_quat).rotation()
p_WS = body1_pos + body1_rotmat.dot(p_AS)
p_WT = body2_pos + body2_rotmat.dot(p_BT)
p_ST_W = p_WT - p_WS
n_W = body1_rotmat.dot(n_A)
self.assertGreater(p_ST_W.dot(n_W), np.linalg.norm(
p_ST_W) * np.linalg.norm(n_W) *
math.cos(cone_half_angle) - 1E-6)
self.assertEqual(len(w), 2)
def test_pose_selector(self):
kScanDistance = 4.
rg = make_two_lane_road()
lane = rg.junction(0).segment(0).lane(0)
pose0 = PoseVector()
pose0.set_translation([1., 0., 0.])
pose1 = PoseVector()
# N.B. Set pose1 3 meters ahead of pose0.
pose1.set_translation([4., 0., 0.])
bundle = PoseBundle(num_poses=2)
bundle.set_pose(0, Isometry3(Quaternion(), pose0.get_translation()))
bundle.set_pose(1, Isometry3(Quaternion(), pose1.get_translation()))
closest_pose = PoseSelector.FindSingleClosestPose(
lane=lane,
ego_pose=pose0,
traffic_poses=bundle,
scan_distance=kScanDistance,
side=AheadOrBehind.kAhead,
path_or_branches=ScanStrategy.kPath)
self.assertEqual(closest_pose.odometry.lane.id().string(),
lane.id().string())
self.assertTrue(closest_pose.distance == 3.)
closest_pair = PoseSelector.FindClosestPair(
lane=lane,
ego_pose=pose0,
traffic_poses=bundle,
scan_distance=kScanDistance,
path_or_branches=ScanStrategy.kPath)
self.assertEqual(
closest_pair[AheadOrBehind.kAhead].odometry.lane.id().string(),
lane.id().string())
self.assertEqual(closest_pair[AheadOrBehind.kAhead].distance, 3.)
self.assertEqual(
closest_pair[AheadOrBehind.kBehind].odometry.lane.id().string(),
lane.id().string())
self.assertEqual(closest_pair[AheadOrBehind.kBehind].distance,
float('inf'))
lane_pos = LanePosition(s=1., r=0., h=0.)
road_pos = RoadPosition(lane=lane, pos=lane_pos)
w = [1., 2., 3.]
v = [-4., -5., -6.]
frame_velocity = FrameVelocity(SpatialVelocity(w=w, v=v))
road_odom = RoadOdometry(road_position=road_pos,
frame_velocity=frame_velocity)
sigma_v = PoseSelector.GetSigmaVelocity(road_odom)
self.assertEqual(sigma_v, v[0])
def test_pose_vector(self):
value = PoseVector()
self.assertTrue(isinstance(value, BasicVector))
self.assertTrue(isinstance(copy.copy(value), PoseVector))
self.assertTrue(isinstance(value.Clone(), PoseVector))
self.assertEqual(value.size(), PoseVector.kSize)
# - Accessors.
with catch_drake_warnings(expected_count=1):
self.assertTrue(isinstance(value.get_isometry(), Isometry3))
self.assertTrue(isinstance(value.get_transform(), RigidTransform))
self.assertTrue(isinstance(
value.get_rotation(), Quaternion))
self.assertTrue(isinstance(
value.get_translation(), np.ndarray))
# - Value.
with catch_drake_warnings(expected_count=1):
self.assertTrue(np.allclose(
value.get_isometry().matrix(), np.eye(4, 4)))
self.assertTrue(np.allclose(
value.get_transform().GetAsMatrix4(), np.eye(4, 4)))
# - Mutators.
p = [0, 1, 2]
q = Quaternion(wxyz=normalized([0.1, 0.3, 0.7, 0.9]))
X_expected = RigidTransform(quaternion=q, p=p)
value.set_translation(p)
value.set_rotation(q)
with catch_drake_warnings(expected_count=1):
self.assertTrue(np.allclose(
value.get_isometry().matrix(), X_expected.GetAsMatrix4()))
self.assertTrue(np.allclose(
value.get_transform().GetAsMatrix4(), X_expected.GetAsMatrix4()))
# - Ensure ordering is ((px, py, pz), (qw, qx, qy, qz))
vector_expected = np.hstack((p, q.wxyz()))
vector_actual = value.get_value()
self.assertTrue(np.allclose(vector_actual, vector_expected))
# - Fully-parameterized constructor.
value1 = PoseVector(rotation=q, translation=p)
with catch_drake_warnings(expected_count=1):
self.assertTrue(np.allclose(
value1.get_isometry().matrix(), X_expected.GetAsMatrix4()))
self.assertTrue(np.allclose(
value1.get_transform().GetAsMatrix4(), X_expected.GetAsMatrix4()))
# Test mutation via RigidTransform
p2 = [10, 20, 30]
q2 = Quaternion(wxyz=normalized([0.2, 0.3, 0.5, 0.8]))
X2_expected = RigidTransform(quaternion=q2, p=p2)
value.set_transform(X2_expected)
self.assertTrue(np.allclose(
value.get_transform().GetAsMatrix4(), X2_expected.GetAsMatrix4()))
def transform_from_dict(data):
pos, _ = get_data_from_dict(data, ['pos', 'position', 'translation'])
orientation, key = get_data_from_dict(data, ['quat', 'quaternion', 'rpy'])
if key == "rpy":
return RigidTransform(RollPitchYaw(orientation), pos)
else:
return RigidTransform(Quaternion(orientation), pos)
def CalcOutput(self, context, output):
t = context.get_time()
pos_vec = self.traj_pos.value(t)
rot_mat_vec = self.traj_rot.value(t)
rb = RigidTransform(Quaternion(rot_mat_vec), pos_vec)
output.SetFrom(Value[RigidTransform](rb))
def test_pose_bundle(self):
num_poses = 7
bundle = PoseBundle(num_poses)
# - Accessors.
self.assertEqual(bundle.get_num_poses(), num_poses)
self.assertTrue(isinstance(bundle.get_transform(0), RigidTransform))
self.assertTrue(isinstance(bundle.get_velocity(0), FrameVelocity))
# - Mutators.
kIndex = 5
p = [0, 1, 2]
q = Quaternion(wxyz=normalized([0.1, 0.3, 0.7, 0.9]))
pose = RigidTransform(quaternion=q, p=p)
bundle.set_transform(kIndex, pose)
self.assertTrue((bundle.get_transform(kIndex).GetAsMatrix34() ==
pose.GetAsMatrix34()).all())
w = [0.1, 0.3, 0.5]
v = [0., 1., 2.]
frame_velocity = FrameVelocity(SpatialVelocity(w=w, v=v))
bundle.set_velocity(kIndex, frame_velocity)
vel_actual = bundle.get_velocity(kIndex).get_velocity()
self.assertTrue(np.allclose(vel_actual.rotational(), w))
self.assertTrue(np.allclose(vel_actual.translational(), v))
name = "Alice"
bundle.set_name(kIndex, name)
self.assertEqual(bundle.get_name(kIndex), name)
instance_id = 42 # Supply a random instance id.
bundle.set_model_instance_id(kIndex, instance_id)
self.assertEqual(bundle.get_model_instance_id(kIndex), instance_id)
def abstract_handler(self, msg, name, lcmMessage):
'''
Function for handling LCM messages originating from a different
channel than the main one
msg: the returning LCM message
field: the field name/path
name: name of corresponding shape
x, y, z: indices for each of the 3 dimensions of the location where
the shape is to be drawn. When the shape is an axis then x
will be the first index of the quaternion array and the other
3 will be the next indices in sequence
'''
self.handle_checkBox(name)
attribute = msg
field = lcmMessage.field
# if there is a %d in the field replace it with the actual index
if ("%d" in field):
arr = self.getVector(attribute, lcmMessage.index_field)
if (lcmMessage.index_element in arr):
i = arr.index(lcmMessage.index_element)
field = field.replace("%d", str(i))
if ("%d" not in field):
# parse field to get the appropriate information. This is done by
# recursively searching for the right attribute in the given message's field
attribute = self.getVector(attribute, field)
currShape = self.shapes[name]
x = lcmMessage.x
# special case of an axis data
if (currShape.type == "axes"):
# get quaternion array, normalize it, and get the corresponding rotation matrix
quaternion = []
for i in range(x, x + 4):
quaternion.append(attribute[i])
norm = np.linalg.norm(quaternion)
quaternion = [x / norm for x in quaternion]
rot_matrix = Quaternion(quaternion).rotation()
pt_world = self.plant.CalcPointsPositions(
self.context, self.plant.GetFrameByName(currShape.frame),
currShape.point, self.plant.world_frame())
next_loc = pt_world.transpose()[0]
self.drawShape(self.shapes[name],
next_loc,
msg,
rotation_matrix=rot_matrix)
else:
next_loc = [attribute[x], attribute[x + 1], attribute[x + 2]]
self.drawShape(self.shapes[name], next_loc, msg)
else:
print(lcmMessage.index_element + "is not present in " +
lcmMessage.index_field + " and so it cannot be visualized")
print("")
def test_AddGazeTargetConstraint(self):
p_AS = np.array([0.1, 0.2, 0.3])
n_A = np.array([0.3, 0.5, 1.2])
p_BT = np.array([1.1, 0.2, 1.5])
cone_half_angle = 0.2 * math.pi
self.ik_two_bodies.AddGazeTargetConstraint(
frameA=self.body1_frame,
p_AS=p_AS,
n_A=n_A,
frameB=self.body2_frame,
p_BT=p_BT,
cone_half_angle=cone_half_angle)
result = self.prog.Solve()
self.assertEqual(result, mp.SolutionResult.kSolutionFound)
q_val = self.prog.GetSolution(self.q)
body1_quat = self._body1_quat(q_val)
body1_pos = self._body1_xyz(q_val)
body2_quat = self._body2_quat(q_val)
body2_pos = self._body2_xyz(q_val)
body1_rotmat = Quaternion(body1_quat).rotation()
body2_rotmat = Quaternion(body2_quat).rotation()
p_WS = body1_pos + body1_rotmat.dot(p_AS)
p_WT = body2_pos + body2_rotmat.dot(p_BT)
p_ST_W = p_WT - p_WS
n_W = body1_rotmat.dot(n_A)
self.assertGreater(
p_ST_W.dot(n_W),
np.linalg.norm(p_ST_W) * np.linalg.norm(n_W) *
math.cos(cone_half_angle) - 1E-6)
def test_roll_pitch_yaw(self):
# - Constructors.
rpy = mut.RollPitchYaw(rpy=[0, 0, 0])
self.assertTrue(
np.allclose(mut.RollPitchYaw(other=rpy).vector(), [0, 0, 0]))
self.assertTrue(np.allclose(rpy.vector(), [0, 0, 0]))
rpy = mut.RollPitchYaw(roll=0, pitch=0, yaw=0)
self.assertTupleEqual(
(rpy.roll_angle(), rpy.pitch_angle(), rpy.yaw_angle()), (0, 0, 0))
rpy = mut.RollPitchYaw(R=mut.RotationMatrix())
self.assertTrue(np.allclose(rpy.vector(), [0, 0, 0]))
rpy = mut.RollPitchYaw(matrix=np.eye(3))
self.assertTrue(np.allclose(rpy.vector(), [0, 0, 0]))
q_I = Quaternion()
rpy_q_I = mut.RollPitchYaw(quaternion=q_I)
self.assertTrue(np.allclose(rpy_q_I.vector(), [0, 0, 0]))
# - Additional properties.
self.assertTrue(np.allclose(rpy.ToQuaternion().wxyz(), q_I.wxyz()))
R = rpy.ToRotationMatrix().matrix()
self.assertTrue(np.allclose(R, np.eye(3)))
# - Converting changes in orientation
self.assertTrue(
np.allclose(rpy.CalcRotationMatrixDt(rpyDt=[0, 0, 0]),
np.zeros((3, 3))))
self.assertTrue(
np.allclose(
rpy.CalcAngularVelocityInParentFromRpyDt(rpyDt=[0, 0, 0]),
[0, 0, 0]))
self.assertTrue(
np.allclose(
rpy.CalcAngularVelocityInChildFromRpyDt(rpyDt=[0, 0, 0]),
[0, 0, 0]))
self.assertTrue(
np.allclose(
rpy.CalcRpyDtFromAngularVelocityInParent(w_AD_A=[0, 0, 0]),
[0, 0, 0]))
self.assertTrue(
np.allclose(
rpy.CalcRpyDDtFromRpyDtAndAngularAccelInParent(
rpyDt=[0, 0, 0], alpha_AD_A=[0, 0, 0]), [0, 0, 0]))
self.assertTrue(
np.allclose(
rpy.CalcRpyDDtFromAngularAccelInChild(rpyDt=[0, 0, 0],
alpha_AD_D=[0, 0, 0]),
[0, 0, 0]))
def test_roll_pitch_yaw(self):
# - Constructors.
rpy = mut.RollPitchYaw(rpy=[0, 0, 0])
self.assertTrue(
np.allclose(mut.RollPitchYaw(other=rpy).vector(), [0, 0, 0]))
self.assertTrue(np.allclose(rpy.vector(), [0, 0, 0]))
rpy = mut.RollPitchYaw(roll=0, pitch=0, yaw=0)
self.assertTupleEqual(
(rpy.roll_angle(), rpy.pitch_angle(), rpy.yaw_angle()), (0, 0, 0))
rpy = mut.RollPitchYaw(R=mut.RotationMatrix())
self.assertTrue(np.allclose(rpy.vector(), [0, 0, 0]))
q_I = Quaternion()
rpy_q_I = mut.RollPitchYaw(quaternion=q_I)
self.assertTrue(np.allclose(rpy_q_I.vector(), [0, 0, 0]))
# - Additional properties.
self.assertTrue(np.allclose(rpy.ToQuaternion().wxyz(), q_I.wxyz()))
R = rpy.ToRotationMatrix().matrix()
self.assertTrue(np.allclose(R, np.eye(3)))
def test_roll_pitch_yaw(self):
# - Constructors.
rpy = mut.RollPitchYaw(rpy=[0, 0, 0])
self.assertTrue(np.allclose(
mut.RollPitchYaw(other=rpy).vector(), [0, 0, 0]))
self.assertTrue(np.allclose(rpy.vector(), [0, 0, 0]))
rpy = mut.RollPitchYaw(roll=0, pitch=0, yaw=0)
self.assertTupleEqual(
(rpy.roll_angle(), rpy.pitch_angle(), rpy.yaw_angle()),
(0, 0, 0))
rpy = mut.RollPitchYaw(R=mut.RotationMatrix())
self.assertTrue(np.allclose(rpy.vector(), [0, 0, 0]))
q_I = Quaternion()
rpy_q_I = mut.RollPitchYaw(quaternion=q_I)
self.assertTrue(np.allclose(rpy_q_I.vector(), [0, 0, 0]))
# - Additional properties.
self.assertTrue(np.allclose(rpy.ToQuaternion().wxyz(), q_I.wxyz()))
R = rpy.ToRotationMatrix().matrix()
self.assertTrue(np.allclose(R, np.eye(3)))
def test_AddOrientationConstraint(self):
theta_bound = 0.2 * math.pi
R_AbarA = RotationMatrix(quaternion=Quaternion(0.5, -0.5, 0.5, 0.5))
R_BbarB = RotationMatrix(quaternion=Quaternion(1.0 / 3, 2.0 /
3, 0, 2.0 / 3))
self.ik_two_bodies.AddOrientationConstraint(frameAbar=self.body1_frame,
R_AbarA=R_AbarA,
frameBbar=self.body2_frame,
R_BbarB=R_BbarB,
theta_bound=theta_bound)
result = mp.Solve(self.prog)
self.assertTrue(result.is_success())
q_val = result.GetSolution(self.q)
body1_quat = self._body1_quat(q_val)
body2_quat = self._body2_quat(q_val)
body1_rotmat = Quaternion(body1_quat).rotation()
body2_rotmat = Quaternion(body2_quat).rotation()
R_AbarBbar = body1_rotmat.transpose().dot(body2_rotmat)
R_AB = R_AbarA.matrix().transpose().dot(
R_AbarBbar.dot(R_BbarB.matrix()))
self.assertGreater(R_AB.trace(), 1 + 2 * math.cos(theta_bound) - 1E-6)
with warnings.catch_warnings(record=True) as w:
warnings.simplefilter('always', DrakeDeprecationWarning)
self.assertEqual(self.prog.Solve(),
mp.SolutionResult.kSolutionFound)
q_val = self.prog.GetSolution(self.q)
body1_quat = self._body1_quat(q_val)
body2_quat = self._body2_quat(q_val)
body1_rotmat = Quaternion(body1_quat).rotation()
body2_rotmat = Quaternion(body2_quat).rotation()
R_AbarBbar = body1_rotmat.transpose().dot(body2_rotmat)
R_AB = R_AbarA.matrix().transpose().dot(
R_AbarBbar.dot(R_BbarB.matrix()))
self.assertGreater(R_AB.trace(),
1 + 2 * math.cos(theta_bound) - 1E-6)
self.assertEqual(len(w), 2)
def test_AddOrientationConstraint(self):
theta_bound = 0.2 * math.pi
R_AbarA = RotationMatrix(quaternion=Quaternion(0.5, -0.5, 0.5, 0.5))
R_BbarB = RotationMatrix(
quaternion=Quaternion(1.0 / 3, 2.0 / 3, 0, 2.0 / 3))
self.ik_two_bodies.AddOrientationConstraint(
frameAbar=self.body1_frame, R_AbarA=R_AbarA,
frameBbar=self.body2_frame, R_BbarB=R_BbarB,
theta_bound=theta_bound)
result = mp.Solve(self.prog)
self.assertTrue(result.is_success())
q_val = result.GetSolution(self.q)
body1_quat = self._body1_quat(q_val)
body2_quat = self._body2_quat(q_val)
body1_rotmat = Quaternion(body1_quat).rotation()
body2_rotmat = Quaternion(body2_quat).rotation()
R_AbarBbar = body1_rotmat.transpose().dot(body2_rotmat)
R_AB = R_AbarA.matrix().transpose().dot(
R_AbarBbar.dot(R_BbarB.matrix()))
self.assertGreater(R_AB.trace(), 1 + 2 * math.cos(theta_bound) - 1E-6)
with warnings.catch_warnings(record=True) as w:
warnings.simplefilter('always', DrakeDeprecationWarning)
self.assertEqual(
self.prog.Solve(), mp.SolutionResult.kSolutionFound)
q_val = self.prog.GetSolution(self.q)
body1_quat = self._body1_quat(q_val)
body2_quat = self._body2_quat(q_val)
body1_rotmat = Quaternion(body1_quat).rotation()
body2_rotmat = Quaternion(body2_quat).rotation()
R_AbarBbar = body1_rotmat.transpose().dot(body2_rotmat)
R_AB = R_AbarA.matrix().transpose().dot(
R_AbarBbar.dot(R_BbarB.matrix()))
self.assertGreater(R_AB.trace(),
1 + 2 * math.cos(theta_bound) - 1E-6)
self.assertEqual(len(w), 2)
def _to_pose(position, quaternion):
"""Given pose parts of an lcmt_viewer_geometry_data, parse it into a
RigidTransform."""
(p_x, p_y, p_z) = position
(q_w, q_x, q_y, q_z) = quaternion
return RigidTransform(R=RotationMatrix(quaternion=Quaternion(
w=q_w,
x=q_x,
y=q_y,
z=q_z,
), ),
p=(p_x, p_y, p_z))
def test_AddPositionConstraint(self):
p_BQ = np.array([0.2, 0.3, 0.5])
p_AQ_lower = np.array([-0.1, -0.2, -0.3])
p_AQ_upper = np.array([-0.05, -0.12, -0.28])
self.ik_two_bodies.AddPositionConstraint(
frameB=self.body1_frame, p_BQ=p_BQ,
frameA=self.body2_frame,
p_AQ_lower=p_AQ_lower, p_AQ_upper=p_AQ_upper)
result = mp.Solve(self.prog)
self.assertTrue(result.is_success())
q_val = result.GetSolution(self.q)
body1_quat = self._body1_quat(q_val)
body1_pos = self._body1_xyz(q_val)
body2_quat = self._body2_quat(q_val)
body2_pos = self._body2_xyz(q_val)
body1_rotmat = Quaternion(body1_quat).rotation()
body2_rotmat = Quaternion(body2_quat).rotation()
p_AQ = body2_rotmat.transpose().dot(
body1_rotmat.dot(p_BQ) + body1_pos - body2_pos)
self.assertTrue(np.greater(p_AQ, p_AQ_lower -
1E-6 * np.ones((3, 1))).all())
self.assertTrue(np.less(p_AQ, p_AQ_upper +
1E-6 * np.ones((3, 1))).all())
with catch_drake_warnings(expected_count=2):
self.assertEqual(
self.prog.Solve(), mp.SolutionResult.kSolutionFound)
self.assertTrue(np.allclose(
self.prog.GetSolution(self.q), q_val))
def test_AddAngleBetweenVectorsConstraint(self):
na_A = np.array([0.2, -0.4, 0.9])
nb_B = np.array([1.4, -0.1, 1.8])
angle_lower = 0.2 * math.pi
angle_upper = 0.2 * math.pi
self.ik_two_bodies.AddAngleBetweenVectorsConstraint(
frameA=self.body1_frame, na_A=na_A,
frameB=self.body2_frame, nb_B=nb_B,
angle_lower=angle_lower, angle_upper=angle_upper)
result = mp.Solve(self.prog)
self.assertTrue(result.is_success())
q_val = result.GetSolution(self.q)
body1_quat = self._body1_quat(q_val)
body2_quat = self._body2_quat(q_val)
body1_rotmat = Quaternion(body1_quat).rotation()
body2_rotmat = Quaternion(body2_quat).rotation()
na_W = body1_rotmat.dot(na_A)
nb_W = body2_rotmat.dot(nb_B)
angle = math.acos(na_W.transpose().dot(nb_W)
/ (np.linalg.norm(na_W) * np.linalg.norm(nb_W)))
self.assertLess(math.fabs(angle - angle_lower), 1E-6)
result = mp.Solve(self.prog)
self.assertTrue(result.is_success())
self.assertTrue(np.allclose(result.GetSolution(self.q), q_val))
def test_AddPointToPointDistanceConstraint(self):
p_B1P1 = np.array([0.2, -0.4, 0.9])
p_B2P2 = np.array([1.4, -0.1, 1.8])
distance_lower = 0.1
distance_upper = 0.2
self.ik_two_bodies.AddPointToPointDistanceConstraint(
frame1=self.body1_frame, p_B1P1=p_B1P1,
frame2=self.body2_frame, p_B2P2=p_B2P2,
distance_lower=distance_lower, distance_upper=distance_upper)
result = mp.Solve(self.prog)
self.assertTrue(result.is_success())
q_val = result.GetSolution(self.q)
body1_quat = self._body1_quat(q_val)
body2_quat = self._body2_quat(q_val)
body1_rotmat = Quaternion(body1_quat).rotation()
body2_rotmat = Quaternion(body2_quat).rotation()
p_WP1 = self._body1_xyz(q_val) + body1_rotmat.dot(p_B1P1)
p_WP2 = self._body2_xyz(q_val) + body2_rotmat.dot(p_B2P2)
distance = np.linalg.norm(p_WP1 - p_WP2)
self.assertLess(distance, distance_upper + 3e-6)
self.assertGreater(distance, distance_lower - 3e-6)
result = mp.Solve(self.prog)
self.assertTrue(result.is_success())
self.assertTrue(np.allclose(result.GetSolution(self.q), q_val))
def test_AddPositionConstraint(self):
p_BQ = np.array([0.2, 0.3, 0.5])
p_AQ_lower = np.array([-0.1, -0.2, -0.3])
p_AQ_upper = np.array([-0.05, -0.12, -0.28])
self.ik_two_bodies.AddPositionConstraint(
frameB=self.body1_frame, p_BQ=p_BQ,
frameA=self.body2_frame,
p_AQ_lower=p_AQ_lower, p_AQ_upper=p_AQ_upper)
result = mp.Solve(self.prog)
self.assertTrue(result.is_success())
q_val = result.GetSolution(self.q)
body1_quat = self._body1_quat(q_val)
body1_pos = self._body1_xyz(q_val)
body2_quat = self._body2_quat(q_val)
body2_pos = self._body2_xyz(q_val)
body1_rotmat = Quaternion(body1_quat).rotation()
body2_rotmat = Quaternion(body2_quat).rotation()
p_AQ = body2_rotmat.transpose().dot(
body1_rotmat.dot(p_BQ) + body1_pos - body2_pos)
self.assertTrue(np.greater(
p_AQ, p_AQ_lower - 1E-6 * np.ones((3, 1))).all())
self.assertTrue(np.less(
p_AQ, p_AQ_upper + 1E-6 * np.ones((3, 1))).all())
result = mp.Solve(self.prog)
self.assertTrue(result.is_success())
self.assertTrue(np.allclose(result.GetSolution(self.q), q_val))
np.testing.assert_array_equal(self.plant.GetPositions(
self.ik_two_bodies.context()), q_val)
self.assertIs(self.ik_two_bodies.get_mutable_context(),
self.ik_two_bodies.context())
def test_AddOrientationConstraint(self):
theta_bound = 0.2 * math.pi
R_AbarA = RotationMatrix(quaternion=Quaternion(0.5, -0.5, 0.5, 0.5))
R_BbarB = RotationMatrix(
quaternion=Quaternion(1.0 / 3, 2.0 / 3, 0, 2.0 / 3))
self.ik_two_bodies.AddOrientationConstraint(
frameAbar=self.body1_frame, R_AbarA=R_AbarA,
frameBbar=self.body2_frame, R_BbarB=R_BbarB,
theta_bound=theta_bound)
result = self.prog.Solve()
self.assertEqual(result, mp.SolutionResult.kSolutionFound)
q_val = self.prog.GetSolution(self.q)
body1_quat = self._body1_quat(q_val)
body2_quat = self._body2_quat(q_val)
body1_rotmat = Quaternion(body1_quat).rotation()
body2_rotmat = Quaternion(body2_quat).rotation()
R_AbarBbar = body1_rotmat.transpose().dot(body2_rotmat)
R_AB = R_AbarA.matrix().transpose().dot(
R_AbarBbar.dot(R_BbarB.matrix()))
self.assertGreater(R_AB.trace(), 1 + 2 * math.cos(theta_bound) - 1E-6)
def get_q_object_final(self, q_object_init, ee_init, ee_final):
"""
Input:
q_object_init: [quaternion, xyz]
ee_init: [x y z r p y]
ee_final: [x y z r p y]
xyz_iiwa: [x, y, z]
Return:
q_object_final: [quaternion, xyz]
"""
xyz_obj = np.array(q_object_init[4:]).transpose()
# xyz_obj[0] -= np.array(xyz_iiwa) # convert translation from world frame to robot base frame
quaternion_obj = Quaternion(np.array(q_object_init[:4]))
rot_obj = RotationMatrix(quaternion_obj)
X_WO = RigidTransform(rot_obj, xyz_obj)
xyz_ee_init = np.array(ee_init[:3]).transpose()
rot_ee_init = RollPitchYaw(ee_init[3], ee_init[4], ee_init[5]).ToRotationMatrix()
X_WE1 = RigidTransform(rot_ee_init, xyz_ee_init)
xyz_ee_final = np.array(ee_final[:3]).transpose()
rot_ee_final = RollPitchYaw(ee_final[3], ee_final[4], ee_final[5]).ToRotationMatrix()
X_WE2 = RigidTransform(rot_ee_final, xyz_ee_final)
X_EO = X_WE1.inverse().multiply(X_WO)
X_WO2 = X_WE2.multiply(X_EO)
wxyz_obj_final = X_WO2.rotation().ToQuaternion().wxyz()
xyz_obj_final = X_WO2.translation()
# xyz_ee_obj_init = xyz_obj - xyz_ee_init
# rot_ee_init_final = rot_ee_init.multiply(rot_ee_final.transpose())
# xyz_ee_obj_final = np.matmul(rot_ee_init_final.matrix(), xyz_ee_obj_init)
# # xyz_ee_obj_final += np.array(xyz_iiwa) # convert translation from robot base frame to world frame
# xyz_obj_final = xyz_ee_final + xyz_ee_obj_final
# rot_obj_final = rot_ee_init_final.multiply(rot_obj)
# quaternion_obj_final = rot_obj_final.ToQuaternion()
# wxyz_obj_final = quaternion_obj_final.wxyz()
q_object_final = []
for i in range(4):
q_object_final.append(wxyz_obj_final[i])
for i in range(3):
q_object_final.append(xyz_obj_final[i])
return q_object_final
def test_roll_pitch_yaw(self):
# - Constructors.
rpy = mut.RollPitchYaw(rpy=[0, 0, 0])
self.assertTrue(np.allclose(
mut.RollPitchYaw(other=rpy).vector(), [0, 0, 0]))
self.assertTrue(np.allclose(rpy.vector(), [0, 0, 0]))
rpy = mut.RollPitchYaw(roll=0, pitch=0, yaw=0)
self.assertTupleEqual(
(rpy.roll_angle(), rpy.pitch_angle(), rpy.yaw_angle()),
(0, 0, 0))
rpy = mut.RollPitchYaw(R=mut.RotationMatrix())
self.assertTrue(np.allclose(rpy.vector(), [0, 0, 0]))
rpy = mut.RollPitchYaw(matrix=np.eye(3))
self.assertTrue(np.allclose(rpy.vector(), [0, 0, 0]))
q_I = Quaternion()
rpy_q_I = mut.RollPitchYaw(quaternion=q_I)
self.assertTrue(np.allclose(rpy_q_I.vector(), [0, 0, 0]))
# - Additional properties.
self.assertTrue(np.allclose(rpy.ToQuaternion().wxyz(), q_I.wxyz()))
R = rpy.ToRotationMatrix().matrix()
self.assertTrue(np.allclose(R, np.eye(3)))
# - Converting changes in orientation
self.assertTrue(np.allclose(rpy.CalcRotationMatrixDt(rpyDt=[0, 0, 0]),
np.zeros((3, 3))))
self.assertTrue(np.allclose(
rpy.CalcAngularVelocityInParentFromRpyDt(rpyDt=[0, 0, 0]),
[0, 0, 0]))
self.assertTrue(np.allclose(
rpy.CalcAngularVelocityInChildFromRpyDt(rpyDt=[0, 0, 0]),
[0, 0, 0]))
self.assertTrue(np.allclose(
rpy.CalcRpyDtFromAngularVelocityInParent(w_AD_A=[0, 0, 0]),
[0, 0, 0]))
self.assertTrue(np.allclose(
rpy.CalcRpyDDtFromRpyDtAndAngularAccelInParent(
rpyDt=[0, 0, 0], alpha_AD_A=[0, 0, 0]), [0, 0, 0]))
self.assertTrue(np.allclose(rpy.CalcRpyDDtFromAngularAccelInChild(
rpyDt=[0, 0, 0], alpha_AD_D=[0, 0, 0]), [0, 0, 0]))
def test_idm_controller(self):
rg = make_two_lane_road()
idm = IdmController(
road=rg,
path_or_branches=ScanStrategy.kPath,
road_position_strategy=RoadPositionStrategy.kExhaustiveSearch,
period_sec=0.)
context = idm.CreateDefaultContext()
output = idm.AllocateOutput()
# Fix the inputs.
pose_vector1 = PoseVector()
pose_vector1.set_translation([1., 2., 3.])
ego_pose_index = idm.ego_pose_input().get_index()
context.FixInputPort(ego_pose_index, pose_vector1)
w = [0., 0., 0.]
v = [1., 0., 0.]
frame_velocity1 = FrameVelocity(SpatialVelocity(w=w, v=v))
ego_velocity_index = idm.ego_velocity_input().get_index()
context.FixInputPort(ego_velocity_index, frame_velocity1)
pose_vector2 = PoseVector()
pose_vector2.set_translation([6., 0., 0.])
bundle = PoseBundle(num_poses=1)
bundle.set_pose(
0, Isometry3(Quaternion(), pose_vector2.get_translation()))
traffic_index = idm.traffic_input().get_index()
context.FixInputPort(traffic_index,
framework.AbstractValue.Make(bundle))
# Verify the inputs.
pose_vector_eval = idm.EvalVectorInput(context, 0)
self.assertTrue(
np.allclose(pose_vector1.get_translation(),
pose_vector_eval.get_translation()))
frame_velocity_eval = idm.EvalVectorInput(context, 1)
self.assertTrue(
np.allclose(frame_velocity1.get_velocity().translational(),
frame_velocity_eval.get_velocity().translational()))
self.assertTrue(
np.allclose(frame_velocity1.get_velocity().rotational(),
frame_velocity_eval.get_velocity().rotational()))
# Verify the outputs.
idm.CalcOutput(context, output)
accel_command_index = idm.acceleration_output().get_index()
accel = output.get_vector_data(accel_command_index)
self.assertEqual(len(accel.get_value()), 1)
self.assertTrue(accel.get_value() < 0.)
def test_AddOrientationConstraint(self):
theta_bound = 0.2 * math.pi
R_AbarA = RotationMatrix(quaternion=Quaternion(0.5, -0.5, 0.5, 0.5))
R_BbarB = RotationMatrix(
quaternion=Quaternion(1.0 / 3, 2.0 / 3, 0, 2.0 / 3))
self.ik_two_bodies.AddOrientationConstraint(
frameAbar=self.body1_frame, R_AbarA=R_AbarA,
frameBbar=self.body2_frame, R_BbarB=R_BbarB,
theta_bound=theta_bound)
result = mp.Solve(self.prog)
self.assertTrue(result.is_success())
q_val = result.GetSolution(self.q)
body1_quat = self._body1_quat(q_val)
body2_quat = self._body2_quat(q_val)
body1_rotmat = Quaternion(body1_quat).rotation()
body2_rotmat = Quaternion(body2_quat).rotation()
R_AbarBbar = body1_rotmat.transpose().dot(body2_rotmat)
R_AB = R_AbarA.matrix().transpose().dot(
R_AbarBbar.dot(R_BbarB.matrix()))
self.assertGreater(R_AB.trace(), 1 + 2 * math.cos(theta_bound) - 1E-6)
result = mp.Solve(self.prog)
self.assertTrue(result.is_success())
q_val = result.GetSolution(self.q)
body1_quat = self._body1_quat(q_val)
body2_quat = self._body2_quat(q_val)
body1_rotmat = Quaternion(body1_quat).rotation()
body2_rotmat = Quaternion(body2_quat).rotation()
R_AbarBbar = body1_rotmat.transpose().dot(body2_rotmat)
R_AB = R_AbarA.matrix().transpose().dot(
R_AbarBbar.dot(R_BbarB.matrix()))
self.assertGreater(R_AB.trace(),
1 + 2 * math.cos(theta_bound) - 1E-6)
def add_sensor(self, camera_name, sensor_config):
"""
Adds Rgbd sensor to the diagram
:param camera_name:
:type camera_name:
:param sensor_config:
:type sensor_config:
:return:
:rtype:
"""
builder = self.builder
sg = self.sg
width = sensor_config['width']
height = sensor_config['height']
fov_y = sensor_config['fov_y']
z_near = sensor_config['z_near']
z_far = sensor_config['z_far']
renderer_name = self._renderer_name
depth_camera_properties = DepthCameraProperties(
width=width,
height=height,
fov_y=fov_y,
renderer_name=renderer_name,
z_near=z_near,
z_far=z_far)
parent_frame_id = sg.world_frame_id()
# This is in right-down-forward convention
pos = np.array(sensor_config['pos'])
quat_eigen = Quaternion(sensor_config['quat'])
# camera to world transform
T_W_camera = RigidTransform(quaternion=quat_eigen, p=pos)
# add camera system
camera = builder.AddSystem(
RgbdSensor(parent_frame_id,
T_W_camera,
depth_camera_properties,
show_window=False))
builder.Connect(sg.get_query_output_port(),
camera.query_object_input_port())
self._rgbd_sensors[camera_name] = camera
def test_api(self):
plant = MultibodyPlant(time_step=0.01)
model_instance = Parser(plant).AddModelFromFile(FindResourceOrThrow(
"drake/bindings/pydrake/multibody/test/two_bodies.sdf"))
plant.Finalize()
context = plant.CreateDefaultContext()
options = ik.GlobalInverseKinematics.Options()
global_ik = ik.GlobalInverseKinematics(plant=plant, options=options)
self.assertIsInstance(global_ik.prog(), mp.MathematicalProgram)
self.assertIsInstance(global_ik.get_mutable_prog(),
mp.MathematicalProgram)
body_index_A = plant.GetBodyIndices(model_instance)[0]
body_index_B = plant.GetBodyIndices(model_instance)[1]
self.assertEqual(
global_ik.body_rotation_matrix(body_index=body_index_A).shape,
(3, 3))
self.assertEqual(
global_ik.body_position(body_index=body_index_A).shape, (3, ))
global_ik.AddWorldPositionConstraint(
body_index=body_index_A,
p_BQ=[0, 0, 0],
box_lb_F=[-np.inf, -np.inf, -np.inf],
box_ub_F=[np.inf, np.inf, np.inf],
X_WF=RigidTransform())
global_ik.AddWorldRelativePositionConstraint(
body_index_B=body_index_B,
p_BQ=[0, 0, 0],
body_index_A=body_index_A,
p_AP=[0, 0, 0],
box_lb_F=[-np.inf, -np.inf, -np.inf],
box_ub_F=[np.inf, np.inf, np.inf],
X_WF=RigidTransform())
global_ik.AddWorldOrientationConstraint(
body_index=body_index_A,
desired_orientation=Quaternion(),
angle_tol=np.inf)
global_ik.AddPostureCost(
q_desired=plant.GetPositions(context),
body_position_cost=[1] * plant.num_bodies(),
body_orientation_cost=[1] * plant.num_bodies())
gurobi_solver = GurobiSolver()
if gurobi_solver.available():
global_ik.SetInitialGuess(q=plant.GetPositions(context))
result = gurobi_solver.Solve(global_ik.prog())
self.assertTrue(result.is_success())
global_ik.ReconstructGeneralizedPositionSolution(result=result)
def test_rigid_transform(self):
def check_equality(X_actual, X_expected_matrix):
# TODO(eric.cousineau): Use `IsNearlyEqualTo`.
self.assertIsInstance(X_actual, mut.RigidTransform)
self.assertTrue(
np.allclose(X_actual.GetAsMatrix4(), X_expected_matrix))
# - Constructors.
X_I = np.eye(4)
check_equality(mut.RigidTransform(), X_I)
check_equality(mut.RigidTransform(other=mut.RigidTransform()), X_I)
check_equality(copy.copy(mut.RigidTransform()), X_I)
R_I = mut.RotationMatrix()
p_I = np.zeros(3)
rpy_I = mut.RollPitchYaw(0, 0, 0)
quaternion_I = Quaternion.Identity()
angle = np.pi * 0
axis = [0, 0, 1]
angle_axis = AngleAxis(angle=angle, axis=axis)
check_equality(mut.RigidTransform(R=R_I, p=p_I), X_I)
check_equality(mut.RigidTransform(rpy=rpy_I, p=p_I), X_I)
check_equality(mut.RigidTransform(quaternion=quaternion_I, p=p_I), X_I)
check_equality(mut.RigidTransform(theta_lambda=angle_axis, p=p_I), X_I)
check_equality(mut.RigidTransform(R=R_I), X_I)
check_equality(mut.RigidTransform(p=p_I), X_I)
# - Accessors, mutators, and general methods.
X = mut.RigidTransform()
X.set(R=R_I, p=p_I)
X.SetFromIsometry3(pose=Isometry3.Identity())
check_equality(mut.RigidTransform.Identity(), X_I)
self.assertIsInstance(X.rotation(), mut.RotationMatrix)
X.set_rotation(R=R_I)
self.assertIsInstance(X.translation(), np.ndarray)
X.set_translation(p=np.zeros(3))
self.assertTrue(np.allclose(X.GetAsMatrix4(), X_I))
self.assertTrue(np.allclose(X.GetAsMatrix34(), X_I[:3]))
self.assertIsInstance(X.GetAsIsometry3(), Isometry3)
check_equality(X.inverse(), X_I)
self.assertIsInstance(
X.multiply(other=mut.RigidTransform()), mut.RigidTransform)
self.assertIsInstance(X.multiply(p_BoQ_B=p_I), np.ndarray)
if six.PY3:
self.assertIsInstance(
eval("X @ mut.RigidTransform()"), mut.RigidTransform)
self.assertIsInstance(eval("X @ [0, 0, 0]"), np.ndarray)
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 __init__(self, server, tf_buffer, joints):
super().__init__()
# TODO(sloretz) do I need to store allllll of this in the context?
# if so, how tf do I get the context in callbacks?
self._tf_buffer = tf_buffer
# System will publish transforms for these joints only
self._joints = tuple(joints)
if 0 == len(self._joints):
raise ValueError('Need at least one joint')
for joint in self._joints:
if not Joint_.is_subclass_of_instantiation(type(joint)):
raise TypeError('joints must be an iterable of Joint_[T]')
self._server = server
self._joint_states = [0.0] * len(self._joints)
# Map joint names to indexes in joint_states
self._joint_indexes = {j.name(): i for j, i in zip(self._joints, range(len(self._joints)))}
self._joint_prev_orientation = {j.name(): Quaternion() for j in self._joints}
self._joint_axis_in_child = {}
for joint in self._joints:
if RevoluteJoint_.is_subclass_of_instantiation(type(joint)):
server.insert(
self._make_revolute_marker(joint),
feedback_callback=functools.partial(self._revolute_feedback, joint))
else:
# TODO(sloretz) support more than revolute joints
raise TypeError('joints must be an iterable of RevoluteJoint_[T]')
server.applyChanges()
self.DeclareVectorOutputPort(
'joint_states',
BasicVector_[float](len(self._joint_states)),
self._do_get_joint_states)
def _revolute_feedback(self, joint, feedback):
if feedback.event_type != InteractiveMarkerFeedback.POSE_UPDATE:
return
expected_frame = joint.child_body().body_frame().name()
if expected_frame != feedback.header.frame_id:
# TODO(sloretz) fix tf2_geometry_msgs_py :(
# transformed_point = self._tf_buffer.transform(point_stamped, self._frame_id)
print("TODO accept feedback in different frame")
return
qw = feedback.pose.orientation.w
qx = feedback.pose.orientation.x
qy = feedback.pose.orientation.y
qz = feedback.pose.orientation.z
new_orientation = Quaternion(qw, qx, qy, qz)
prev_orientation = self._joint_prev_orientation[joint.name()]
orientation_diff = prev_orientation.inverse().multiply(new_orientation)
diff_aa = AngleAxis(orientation_diff)
joint_axis = self._joint_axis_in_child[joint.name()]
dot = numpy.dot(joint_axis, diff_aa.axis())
if dot > 0.999:
angle_inc = diff_aa.angle()
elif dot < -0.999:
angle_inc = -1 * diff_aa.angle()
else:
angle_inc = 0.
angle = self._joint_states[self._joint_indexes[joint.name()]] + angle_inc
if angle > joint.position_upper_limit():
angle = joint.position_upper_limit()
elif angle < joint.position_lower_limit():
angle = joint.position_lower_limit()
self._joint_states[self._joint_indexes[joint.name()]] = angle
self._joint_prev_orientation[joint.name()] = new_orientation
def test_quaternion_slerp(self):
# Test empty constructor.
pq = PiecewiseQuaternionSlerp()
self.assertEqual(pq.rows(), 4)
self.assertEqual(pq.cols(), 1)
self.assertEqual(pq.get_number_of_segments(), 0)
t = [0., 1., 2.]
# Make identity rotations.
q = Quaternion()
m = np.identity(3)
a = AngleAxis()
R = RotationMatrix()
# Test quaternion constructor.
pq = PiecewiseQuaternionSlerp(breaks=t, quaternions=[q, q, q])
self.assertEqual(pq.get_number_of_segments(), 2)
np.testing.assert_equal(pq.value(0.5), np.eye(3))
# Test matrix constructor.
pq = PiecewiseQuaternionSlerp(breaks=t, rotation_matrices=[m, m, m])
self.assertEqual(pq.get_number_of_segments(), 2)
np.testing.assert_equal(pq.value(0.5), np.eye(3))
# Test axis angle constructor.
pq = PiecewiseQuaternionSlerp(breaks=t, angle_axes=[a, a, a])
self.assertEqual(pq.get_number_of_segments(), 2)
np.testing.assert_equal(pq.value(0.5), np.eye(3))
# Test rotation matrix constructor.
pq = PiecewiseQuaternionSlerp(breaks=t, rotation_matrices=[R, R, R])
self.assertEqual(pq.get_number_of_segments(), 2)
np.testing.assert_equal(pq.value(0.5), np.eye(3))
# Test append operations.
pq.Append(time=3., quaternion=q)
pq.Append(time=4., rotation_matrix=R)
pq.Append(time=5., angle_axis=a)
def test_AddAngleBetweenVectorsConstraint(self):
na_A = np.array([0.2, -0.4, 0.9])
nb_B = np.array([1.4, -0.1, 1.8])
angle_lower = 0.2 * math.pi
angle_upper = 0.2 * math.pi
self.ik_two_bodies.AddAngleBetweenVectorsConstraint(
frameA=self.body1_frame,
na_A=na_A,
frameB=self.body2_frame,
nb_B=nb_B,
angle_lower=angle_lower,
angle_upper=angle_upper)
result = mp.Solve(self.prog)
self.assertTrue(result.is_success())
q_val = result.GetSolution(self.q)
body1_quat = self._body1_quat(q_val)
body2_quat = self._body2_quat(q_val)
body1_rotmat = Quaternion(body1_quat).rotation()
body2_rotmat = Quaternion(body2_quat).rotation()
na_W = body1_rotmat.dot(na_A)
nb_W = body2_rotmat.dot(nb_B)
angle = math.acos(na_W.transpose().dot(nb_W) /
(np.linalg.norm(na_W) * np.linalg.norm(nb_W)))
self.assertLess(math.fabs(angle - angle_lower), 1E-6)
with warnings.catch_warnings(record=True) as w:
warnings.simplefilter('always', DrakeDeprecationWarning)
self.assertEqual(self.prog.Solve(),
mp.SolutionResult.kSolutionFound)
self.assertTrue(np.allclose(self.prog.GetSolution(self.q), q_val))
self.assertEqual(len(w), 2)
def _convert_mesh(geom):
# Given a LCM geometry message, forms a meshcat geometry and material
# for that geometry, as well as a local tf to the meshcat geometry
# that matches the LCM geometry.
# For MESH geometry, this function checks if a texture file exists next
# to the mesh file, and uses that to provide the material information if
# present. For BOX, SPHERE, CYLINDER geometry as well as MESH geometry
# not satisfying the above, this function uses the geom.color field to
# create a flat color for the object. In the case of other geometry types,
# both fields are returned as None.
meshcat_geom = None
material = None
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")
# Handle scaling.
# TODO(gizatt): See meshcat-python#40 for incorporating scale as a
# field rather than a matrix multiplication.
scale = geom.float_data[:3]
element_local_tf[:3, :3] = element_local_tf[:3, :3].dot(np.diag(scale))
# Attempt to find a texture for the object by looking for an
# identically-named *.png next to the model.
# TODO(gizatt): Support .MTLs and prefer them over png, since they're
# both more expressive and more standard.
# TODO(gizatt): In the long term, this kind of material information
# should be gleaned from the SceneGraph constituents themselves, so
# that we visualize what the simulation is *actually* reasoning about
# rather than what files happen to be present.
candidate_texture_path_png = geom.string_data[0:-3] + "png"
if os.path.exists(candidate_texture_path_png):
material = meshcat.geometry.MeshLambertMaterial(
map=meshcat.geometry.ImageTexture(
image=meshcat.geometry.PngImage.from_file(
candidate_texture_path_png)))
else:
print("UNSUPPORTED GEOMETRY TYPE {} IGNORED".format(
geom.type))
return meshcat_geom, material
if material is None:
def rgb_2_hex(rgb):
# Turn a list of R,G,B elements (any indexable list
# of >= 3 elements will work), where each element is
# specified on range [0., 1.], into the equivalent
# 24-bit value 0xRRGGBB.
val = 0
for i in range(3):
val += (256**(2 - i)) * int(255 * rgb[i])
return val
material = meshcat.geometry.MeshLambertMaterial(
color=rgb_2_hex(geom.color[:3]),
transparent=geom.color[3] != 1.,
opacity=geom.color[3])
return meshcat_geom, material, element_local_tf
def test_quaternion_slerp(self):
# Test empty constructor.
pq = PiecewiseQuaternionSlerp()
self.assertEqual(pq.rows(), 4)
self.assertEqual(pq.cols(), 1)
self.assertEqual(pq.get_number_of_segments(), 0)
t = [0., 1., 2.]
# Make identity rotations.
q = Quaternion()
m = np.identity(3)
a = AngleAxis()
R = RotationMatrix()
# Test quaternion constructor.
pq = PiecewiseQuaternionSlerp(breaks=t, quaternions=[q, q, q])
self.assertEqual(pq.get_number_of_segments(), 2)
np.testing.assert_equal(pq.value(0.5), np.eye(3))
# Test matrix constructor.
pq = PiecewiseQuaternionSlerp(breaks=t, rotation_matrices=[m, m, m])
self.assertEqual(pq.get_number_of_segments(), 2)
np.testing.assert_equal(pq.value(0.5), np.eye(3))
# Test axis angle constructor.
pq = PiecewiseQuaternionSlerp(breaks=t, angle_axes=[a, a, a])
self.assertEqual(pq.get_number_of_segments(), 2)
np.testing.assert_equal(pq.value(0.5), np.eye(3))
# Test rotation matrix constructor.
pq = PiecewiseQuaternionSlerp(breaks=t, rotation_matrices=[R, R, R])
self.assertEqual(pq.get_number_of_segments(), 2)
np.testing.assert_equal(pq.value(0.5), np.eye(3))
# Test append operations.
pq.Append(time=3., quaternion=q)
pq.Append(time=4., rotation_matrix=R)
pq.Append(time=5., angle_axis=a)
# Test getters.
pq = PiecewiseQuaternionSlerp(
breaks=[0, 1], angle_axes=[a, AngleAxis(np.pi / 2, [0, 0, 1])])
np.testing.assert_equal(np.array([1, 0, 0, 0]),
pq.orientation(time=0).wxyz())
np.testing.assert_allclose(
np.array([0, 0, np.pi / 2]),
pq.angular_velocity(time=0),
atol=1e-15,
rtol=0,
)
np.testing.assert_equal(np.zeros(3), pq.angular_acceleration(time=0))
np.testing.assert_allclose(
np.array([np.cos(np.pi / 4), 0, 0,
np.sin(np.pi / 4)]),
pq.orientation(time=1).wxyz(),
atol=1e-15,
rtol=0,
)
np.testing.assert_allclose(
np.array([0, 0, np.pi / 2]),
pq.angular_velocity(time=1),
atol=1e-15,
rtol=0,
)
np.testing.assert_equal(np.zeros(3), pq.angular_acceleration(time=1))
