def test_frame_velocity(self):
frame_velocity = FrameVelocity()
self.assertTrue(isinstance(frame_velocity, BasicVector))
self.assertTrue(isinstance(copy.copy(frame_velocity), FrameVelocity))
self.assertTrue(isinstance(frame_velocity.Clone(), FrameVelocity))
self.assertEqual(frame_velocity.size(), FrameVelocity.kSize)
# - Accessors.
self.assertTrue(
isinstance(frame_velocity.get_velocity(), SpatialVelocity))
# - Value.
self.assertTrue(
np.allclose(frame_velocity.get_velocity().rotational(), 3 * [0.]))
self.assertTrue(
np.allclose(frame_velocity.get_velocity().translational(),
3 * [0.]))
# - Mutators.
w = [0.1, 0.3, 0.5]
v = [0., 1., 2.]
frame_velocity.set_velocity(SpatialVelocity(w=w, v=v))
self.assertTrue(
np.allclose(frame_velocity.get_velocity().rotational(), w))
self.assertTrue(
np.allclose(frame_velocity.get_velocity().translational(), v))
# - Ensure ordering is ((wx, wy, wz), (vx, vy, vz))
velocity_expected = np.hstack((w, v))
velocity_actual = frame_velocity.get_value()
self.assertTrue(np.allclose(velocity_actual, velocity_expected))
# - Fully-parameterized constructor.
frame_velocity1 = FrameVelocity(SpatialVelocity(w=w, v=v))
self.assertTrue(
np.allclose(frame_velocity1.get_velocity().rotational(), w))
self.assertTrue(
np.allclose(frame_velocity1.get_velocity().translational(), v))
def test_road_odometry(self):
RoadOdometry()
rg = make_two_lane_road()
lane_0 = rg.junction(0).segment(0).lane(0)
lane_1 = rg.junction(0).segment(0).lane(1)
lane_pos = LanePosition(s=1., r=2., h=3.)
road_pos = RoadPosition(lane=lane_0, pos=lane_pos)
w = [5., 7., 9.]
v = [11., 13., 15.]
frame_velocity = FrameVelocity(SpatialVelocity(w=w, v=v))
road_odom = RoadOdometry(road_position=road_pos,
frame_velocity=frame_velocity)
self.assertEqual(road_odom.lane.id().string(), lane_0.id().string())
self.assertTrue(np.allclose(road_odom.pos.srh(), lane_pos.srh()))
self.assertTrue(
np.allclose(frame_velocity.get_velocity().translational(),
road_odom.vel.get_velocity().translational()))
lane_pos_new = LanePosition(s=10., r=20., h=30.)
v_new = [42., 43., 44.]
frame_velocity_new = FrameVelocity(SpatialVelocity(w=w, v=v_new))
road_odom.lane = lane_1
road_odom.pos = lane_pos_new
road_odom.vel = frame_velocity_new
self.assertEqual(road_odom.lane.id().string(), lane_1.id().string())
self.assertTrue(np.allclose(road_odom.pos.srh(), lane_pos_new.srh()))
self.assertTrue(
np.allclose(frame_velocity_new.get_velocity().translational(),
road_odom.vel.get_velocity().translational()))
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 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_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 controller(
X_TPdes: RigidTransform,
V_TPdes: SpatialVelocity,
x: VectorArg(nx),
) -> ForceList:
# Cribbed from:
# https://github.com/gizatt/drake_hydra_interact/tree/cce3ecbb
frame_W = plant.world_frame()
plant.SetPositionsAndVelocities(context, model_instance, x)
X_WT = plant.CalcRelativeTransform(context, frame_W, frame_T)
V_WT = me.get_frame_spatial_velocity(plant, context, frame_W, frame_T)
X_WPdes = X_WT @ X_TPdes
V_WPdes = V_WT.ComposeWithMovingFrameVelocity(
X_WT.translation(), V_TPdes.Rotate(X_WT.rotation()))
X_WP = plant.CalcRelativeTransform(context, frame_W, frame_P)
R_WP = X_WP.rotation()
V_WP = me.get_frame_spatial_velocity(plant, context, frame_W, frame_P)
# Transform to "negative error": desired w.r.t. actual,
# expressed in world frame (for applying the force).
p_PPdes_W = X_WPdes.translation() - X_WP.translation()
# N.B. We don't project away symmetry here because we're expecting
# smooth trajectories (for now).
R_PPdes = R_WP.inverse() @ X_WPdes.rotation()
axang3_PPdes = rotation_matrix_to_axang3(R_PPdes)
axang3_PPdes_W = R_WP @ axang3_PPdes
V_PPdes_W = V_WPdes - V_WP
# Compute wrench components.
f_P_W = Kp_xyz @ p_PPdes_W + Kd_xyz @ V_PPdes_W.translational()
tau_P_W = (Kp_rot(R_WP) @ axang3_PPdes_W +
Kd_rot(R_WP) @ V_PPdes_W.rotational())
F_P_W_feedback = SpatialForce(tau=tau_P_W, f=f_P_W)
# Add gravity-compensation term.
g_W = plant.gravity_field().gravity_vector()
F_Pcm_W = SpatialForce(tau=[0, 0, 0], f=-g_W * M_PPo_P.get_mass())
p_PoPcm_W = R_WP @ M_PPo_P.get_com()
F_P_W_feedforward = F_Pcm_W.Shift(-p_PoPcm_W)
# Package it up.
F_P_W = F_P_W_feedback + F_P_W_feedforward
external_force = ExternallyAppliedSpatialForce()
external_force.body_index = frame_P.body().index()
external_force.F_Bq_W = F_P_W
external_force.p_BoBq_B = (
frame_P.GetFixedPoseInBodyFrame().translation())
return ForceList([external_force])
def get_frame_spatial_velocity(plant, context, frame_T, frame_F, frame_E=None):
"""
Returns:
SpatialVelocity of frame F's origin w.r.t. frame T, expressed in E
(which is frame T if unspecified).
"""
if frame_E is None:
frame_E = frame_T
Jv_TF_E = plant.CalcJacobianSpatialVelocity(
context,
with_respect_to=JacobianWrtVariable.kV,
frame_B=frame_F,
p_BP=[0, 0, 0],
frame_A=frame_T,
frame_E=frame_E,
)
v = plant.GetVelocities(context)
V_TF_E = SpatialVelocity(Jv_TF_E @ v)
return V_TF_E
def test_closest_pose(self):
ClosestPose()
rg = make_two_lane_road()
lane_0 = rg.junction(0).segment(0).lane(0)
lane_1 = rg.junction(0).segment(0).lane(1)
lane_pos = LanePosition(s=1., r=2., h=3.)
road_pos = RoadPosition(lane=lane_0, pos=lane_pos)
w = [5., 7., 9.]
v = [11., 13., 15.]
frame_velocity = FrameVelocity(SpatialVelocity(w=w, v=v))
road_odom = RoadOdometry(road_position=road_pos,
frame_velocity=frame_velocity)
closest_pose = ClosestPose(odom=road_odom, dist=12.7)
self.assertEqual(closest_pose.odometry.lane.id().string(),
lane_0.id().string())
self.assertEqual(closest_pose.distance, 12.7)
closest_pose.odometry.lane = lane_1
closest_pose.distance = 5.9
self.assertEqual(closest_pose.odometry.lane.id().string(),
lane_1.id().string())
self.assertEqual(closest_pose.distance, 5.9)
def test_multibody_tree_kinematics(self):
file_name = FindResourceOrThrow(
"drake/examples/double_pendulum/models/double_pendulum.sdf")
plant = MultibodyPlant()
Parser(plant).AddModelFromFile(file_name)
plant.Finalize()
context = plant.CreateDefaultContext()
world_frame = plant.world_frame()
base = plant.GetBodyByName("base")
base_frame = plant.GetFrameByName("base")
X_WL = plant.CalcRelativeTransform(
context, frame_A=world_frame, frame_B=base_frame)
self.assertIsInstance(X_WL, RigidTransform)
p_AQi = plant.CalcPointsPositions(
context=context, frame_B=base_frame,
p_BQi=np.array([[0, 1, 2], [10, 11, 12]]).T,
frame_A=world_frame).T
self.assertTupleEqual(p_AQi.shape, (2, 3))
Jv_WL = plant.CalcFrameGeometricJacobianExpressedInWorld(
context=context, frame_B=base_frame,
p_BoFo_B=[0, 0, 0])
self.assertTupleEqual(Jv_WL.shape, (6, plant.num_velocities()))
nq = plant.num_positions()
nv = plant.num_velocities()
wrt_list = [
(JacobianWrtVariable.kQDot, nq),
(JacobianWrtVariable.kV, nv),
]
for wrt, nw in wrt_list:
Jw_ABp_E = plant.CalcJacobianSpatialVelocity(
context=context, with_respect_to=wrt, frame_B=base_frame,
p_BP=np.zeros(3), frame_A=world_frame,
frame_E=world_frame)
self.assert_sane(Jw_ABp_E)
self.assertEqual(Jw_ABp_E.shape, (6, nw))
# Compute body pose.
X_WBase = plant.EvalBodyPoseInWorld(context, base)
self.assertIsInstance(X_WBase, RigidTransform)
# Set pose for the base.
X_WB_desired = RigidTransform.Identity()
X_WB = plant.CalcRelativeTransform(context, world_frame, base_frame)
plant.SetFreeBodyPose(
context=context, body=base, X_WB=X_WB_desired)
self.assertTrue(np.allclose(X_WB.matrix(), X_WB_desired.matrix()))
# Set a spatial velocity for the base.
v_WB = SpatialVelocity(w=[1, 2, 3], v=[4, 5, 6])
plant.SetFreeBodySpatialVelocity(
context=context, body=base, V_WB=v_WB)
v_base = plant.EvalBodySpatialVelocityInWorld(context, base)
self.assertTrue(np.allclose(v_base.rotational(), v_WB.rotational()))
self.assertTrue(np.allclose(v_base.translational(),
v_WB.translational()))
# Compute accelerations.
vdot = np.zeros(nv)
A_WB_array = plant.CalcSpatialAccelerationsFromVdot(
context=context, known_vdot=vdot)
self.assertEqual(len(A_WB_array), plant.num_bodies())
def test_pose_aggregator(self):
aggregator = PoseAggregator()
# - Set-up.
instance_id1 = 5 # Supply a random instance id.
port1 = aggregator.AddSingleInput("pose_only", instance_id1)
self.assertEqual(port1.get_data_type(), PortDataType.kVectorValued)
self.assertEqual(port1.size(), PoseVector.kSize)
instance_id2 = 42 # Supply another random, but unique, id.
ports2 = aggregator.AddSinglePoseAndVelocityInput(
"pose_and_velocity", instance_id2)
self.assertEqual(ports2.pose_input_port.get_data_type(),
PortDataType.kVectorValued)
self.assertEqual(ports2.pose_input_port.size(), PoseVector.kSize)
self.assertEqual(ports2.velocity_input_port.get_data_type(),
PortDataType.kVectorValued)
self.assertEqual(ports2.velocity_input_port.size(),
FrameVelocity.kSize)
num_poses = 1
port3 = aggregator.AddBundleInput("pose_bundle", num_poses)
self.assertEqual(port3.get_data_type(), PortDataType.kAbstractValued)
# - CalcOutput.
self.assertEqual(
aggregator.get_output_port(0).get_data_type(),
PortDataType.kAbstractValued)
context = aggregator.CreateDefaultContext()
output = aggregator.AllocateOutput()
p1 = [0, 1, 2]
pose1 = PoseVector()
pose1.set_translation(p1)
p2 = [5, 7, 9]
pose2 = PoseVector()
pose2.set_translation(p2)
w = [0.3, 0.4, 0.5]
v = [0.5, 0.6, 0.7]
velocity = FrameVelocity()
velocity.set_velocity(SpatialVelocity(w=w, v=v))
p3 = [50, 70, 90]
q3 = Quaternion(wxyz=normalized([0.1, 0.3, 0.7, 0.9]))
bundle = PoseBundle(num_poses)
bundle.set_transform(0, RigidTransform(quaternion=q3, p=p3))
bundle_value = AbstractValue.Make(bundle)
aggregator.get_input_port(0).FixValue(context, pose1)
aggregator.get_input_port(1).FixValue(context, pose2)
aggregator.get_input_port(2).FixValue(context, velocity)
aggregator.get_input_port(3).FixValue(context, bundle_value)
aggregator.CalcOutput(context, output)
value = output.get_data(0).get_value()
self.assertEqual(value.get_num_poses(), 3)
pose1_actual = RigidTransform(p=p1)
self.assertTrue((value.get_transform(0).GetAsMatrix34() ==
pose1_actual.GetAsMatrix34()).all())
pose2_actual = RigidTransform(p=p2)
self.assertTrue((value.get_transform(1).GetAsMatrix34() ==
pose2_actual.GetAsMatrix34()).all())
vel_actual = value.get_velocity(1).get_velocity()
self.assertTrue(np.allclose(vel_actual.rotational(), w))
self.assertTrue(np.allclose(vel_actual.translational(), v))
pose3_actual = RigidTransform(quaternion=q3, p=p3)
self.assertTrue((value.get_transform(2).GetAsMatrix34() ==
pose3_actual.GetAsMatrix34()).all())
def test_multibody_tree_kinematics(self):
file_name = FindResourceOrThrow(
"drake/examples/double_pendulum/models/double_pendulum.sdf")
plant = MultibodyPlant()
Parser(plant).AddModelFromFile(file_name)
plant.Finalize()
context = plant.CreateDefaultContext()
world_frame = plant.world_frame()
base = plant.GetBodyByName("base")
base_frame = plant.GetFrameByName("base")
X_WL = plant.CalcRelativeTransform(context,
frame_A=world_frame,
frame_B=base_frame)
self.assertIsInstance(X_WL, RigidTransform)
p_AQi = plant.CalcPointsPositions(context=context,
frame_B=base_frame,
p_BQi=np.array([[0, 1, 2],
[10, 11, 12]]).T,
frame_A=world_frame).T
self.assertTupleEqual(p_AQi.shape, (2, 3))
Jv_WL = plant.CalcFrameGeometricJacobianExpressedInWorld(
context=context, frame_B=base_frame, p_BoFo_B=[0, 0, 0])
self.assertTupleEqual(Jv_WL.shape, (6, plant.num_velocities()))
nq = plant.num_positions()
nv = plant.num_velocities()
wrt_list = [
(JacobianWrtVariable.kQDot, nq),
(JacobianWrtVariable.kV, nv),
]
for wrt, nw in wrt_list:
Jw_ABp_E = plant.CalcJacobianSpatialVelocity(context=context,
with_respect_to=wrt,
frame_B=base_frame,
p_BP=np.zeros(3),
frame_A=world_frame,
frame_E=world_frame)
self.assert_sane(Jw_ABp_E)
self.assertEqual(Jw_ABp_E.shape, (6, nw))
# Compute body pose.
X_WBase = plant.EvalBodyPoseInWorld(context, base)
self.assertIsInstance(X_WBase, RigidTransform)
# Set pose for the base.
X_WB_desired = RigidTransform.Identity()
X_WB = plant.CalcRelativeTransform(context, world_frame, base_frame)
plant.SetFreeBodyPose(context=context, body=base, X_WB=X_WB_desired)
self.assertTrue(np.allclose(X_WB.matrix(), X_WB_desired.matrix()))
# Set a spatial velocity for the base.
v_WB = SpatialVelocity(w=[1, 2, 3], v=[4, 5, 6])
plant.SetFreeBodySpatialVelocity(context=context, body=base, V_WB=v_WB)
v_base = plant.EvalBodySpatialVelocityInWorld(context, base)
self.assertTrue(np.allclose(v_base.rotational(), v_WB.rotational()))
self.assertTrue(
np.allclose(v_base.translational(), v_WB.translational()))
# Compute accelerations.
vdot = np.zeros(nv)
A_WB_array = plant.CalcSpatialAccelerationsFromVdot(context=context,
known_vdot=vdot)
self.assertEqual(len(A_WB_array), plant.num_bodies())