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_pose(0), Isometry3))
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]))
bundle.set_pose(kIndex, Isometry3(q, p))
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)
self.assertTrue(
(bundle.get_pose(kIndex).matrix() == Isometry3(q,
p).matrix()).all())
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_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_set_free_body_pose(self):
file_name = FindResourceOrThrow(
"drake/examples/double_pendulum/models/double_pendulum.sdf")
plant = MultibodyPlant()
plant_model = AddModelFromSdfFile(file_name, plant)
plant.Finalize()
context = plant.CreateDefaultContext()
tree = plant.tree()
X_WB_desired = Isometry3.Identity()
R_WB = np.array([[0., 1., 0.],
[0., 0., 1.],
[1., 0., 0.]])
X_WB_desired.set_rotation(R_WB)
tree.SetFreeBodyPoseOrThrow(
body=plant.GetBodyByName("base", plant_model),
X_WB=X_WB_desired, context=context)
world_frame = plant.world_frame()
base_frame = plant.GetBodyByName("base").body_frame()
X_WB = tree.CalcRelativeTransform(
context, frame_A=world_frame, frame_B=base_frame)
self.assertTrue(np.allclose(X_WB.matrix(), X_WB_desired.matrix()))
def CalcKinematics(self, X_L7E, l7_frame, world_frame, tree_iiwa,
context_iiwa, handle_angle_ref):
# calculate Geometric jacobian (6 by 7 matrix) of point Q in EE frame.
p_L7Q = X_L7E.multiply(p_EQ)
Jv_WL7q = tree_iiwa.CalcFrameGeometricJacobianExpressedInWorld(
context=context_iiwa, frame_B=l7_frame, p_BoFo_B=p_L7Q)
# Translation
# Hr: handle reference frame
X_WHr = Isometry3()
X_WHr.set_rotation(
RollPitchYaw(0, 0, -handle_angle_ref).ToRotationMatrix().matrix())
X_WHr.set_translation(p_WC_left_hinge + [
-r_handle * np.sin(handle_angle_ref), -r_handle *
np.cos(handle_angle_ref), 0
])
X_HrW = X_WHr.inverse()
X_WL7 = tree_iiwa.CalcRelativeTransform(context_iiwa,
frame_A=world_frame,
frame_B=l7_frame)
p_WQ = X_WL7.multiply(p_L7Q)
p_HrQ = X_HrW.multiply(p_WQ)
return Jv_WL7q, p_HrQ
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)
R_I = mut.RotationMatrix()
p_I = np.zeros(3)
check_equality(mut.RigidTransform(R=R_I, 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)
def create_transform(translation=None, rotation=None):
pose = Isometry3.Identity()
if translation is not None:
pose.set_translation(translation)
if rotation is not None:
pose.set_rotation(matrix_from_euler(rotation))
return pose
def __init__(self):
LeafSystem.__init__(self)
self._DeclareAbstractOutputPort(
"X_WE", lambda: AbstractValue.Make(Isometry3.Identity()),
self._DoCalcOutput)
self._DeclarePeriodicPublish(0.1, 0.0)
self.window = tk.Tk()
self.window.title("End-Effector TeleOp")
self.roll = tk.Scale(self.window,
from_=-2 * np.pi,
to=2 * np.pi,
resolution=-1,
label="roll",
length=800,
orient=tk.HORIZONTAL)
self.roll.pack()
self.pitch = tk.Scale(self.window,
from_=-2 * np.pi,
to=2 * np.pi,
resolution=-1,
label="pitch",
length=800,
orient=tk.HORIZONTAL)
self.pitch.pack()
self.yaw = tk.Scale(self.window,
from_=-2 * np.pi,
to=2 * np.pi,
resolution=-1,
label="yaw",
length=800,
orient=tk.HORIZONTAL)
self.yaw.pack()
self.x = tk.Scale(self.window,
from_=0.1,
to=0.8,
resolution=-1,
label="x",
length=800,
orient=tk.HORIZONTAL)
self.x.pack()
self.y = tk.Scale(self.window,
from_=-0.3,
to=0.3,
resolution=-1,
label="y",
length=800,
orient=tk.HORIZONTAL)
self.y.pack()
self.z = tk.Scale(self.window,
from_=0,
to=0.8,
resolution=-1,
label="z",
length=800,
orient=tk.HORIZONTAL)
self.z.pack()
def GetEndEffectorWorldAlignedFrame():
X_EEa = Isometry3.Identity()
X_EEa.set_rotation(np.array([[
0.,
1.,
0,
], [0, 0, 1], [1, 0, 0]]))
return X_EEa
def test_collision_element_api(self):
# Verify construction from both Isometry3d and 4x4 arrays.
box_element = shapes.Box([1.0, 1.0, 1.0])
box_collision_element_np = CollisionElement(box_element, np.eye(4))
box_collision_element_isom = CollisionElement(box_element,
Isometry3.Identity())
body = RigidBody()
box_collision_element_isom.set_body(body)
self.assertEqual(box_collision_element_isom.get_body(), body)
def test_joints_api(self):
# Verify construction from both Isometry3d and 4x4 arrays,
# and sanity-check that the accessors function.
name = "z"
prismatic_joint_np = PrismaticJoint(name, np.eye(4),
np.array([0., 0., 1.]))
prismatic_joint_isom = PrismaticJoint(name, Isometry3.Identity(),
np.array([0., 0., 1.]))
self.assertEqual(prismatic_joint_isom.get_num_positions(), 1)
self.assertEqual(prismatic_joint_isom.get_name(), name)
name = "theta"
revolute_joint_np = RevoluteJoint(name, np.eye(4),
np.array([0., 0., 1.]))
revolute_joint_isom = RevoluteJoint(name, Isometry3.Identity(),
np.array([0., 0., 1.]))
self.assertEqual(revolute_joint_isom.get_num_positions(), 1)
self.assertEqual(revolute_joint_isom.get_name(), name)
def test_visual_element_api(self):
material_in = [0.3, 0.4, 0.5, 0.6]
material_in_2 = [0.6, 0.7, 0.8, 0.9]
box = shapes.Box(size=[1., 1., 1.])
visual_element_np = shapes.VisualElement(box, np.eye(4), material_in)
visual_element_isom = shapes.VisualElement(box, Isometry3.Identity(),
material_in)
self.assertTrue(
np.allclose(visual_element_np.getMaterial(), material_in))
visual_element_np.setMaterial(material_in_2)
self.assertTrue(
np.allclose(visual_element_np.getMaterial(), material_in_2))
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_multibody_add_joint(self):
"""
Tests joint constructors and `AddJoint`.
"""
instance_file = FindResourceOrThrow(
"drake/examples/double_pendulum/models/double_pendulum.sdf")
# Add different joints between multiple model instances.
# TODO(eric.cousineau): Remove the multiple instances and use
# programmatically constructed bodies once this API is exposed in
# Python.
num_joints = 2
plant = MultibodyPlant()
parser = Parser(plant)
instances = []
for i in range(num_joints + 1):
instance = parser.AddModelFromFile(instance_file,
"instance_{}".format(i))
instances.append(instance)
proximal_frame = "base"
distal_frame = "lower_link"
joints = [
RevoluteJoint(
name="revolve_things",
frame_on_parent=plant.GetBodyByName(distal_frame,
instances[1]).body_frame(),
frame_on_child=plant.GetBodyByName(proximal_frame,
instances[2]).body_frame(),
axis=[0, 0, 1],
damping=0.),
WeldJoint(
name="weld_things",
parent_frame_P=plant.GetBodyByName(distal_frame,
instances[0]).body_frame(),
child_frame_C=plant.GetBodyByName(proximal_frame,
instances[1]).body_frame(),
X_PC=Isometry3.Identity()),
]
for joint in joints:
joint_out = plant.AddJoint(joint)
self.assertIs(joint, joint_out)
# The model is now complete.
plant.Finalize()
for joint in joints:
self._test_joint_api(joint)
def test_mbp_overloads(self):
file_name = FindResourceOrThrow(
"drake/multibody/benchmarks/acrobot/acrobot.sdf")
plant = MultibodyPlant()
AddModelFromSdfFile(file_name=file_name, plant=plant, scene_graph=None)
plant.Finalize()
context = plant.CreateDefaultContext()
frame = plant.GetFrameByName("Link2")
parameters = mut.DifferentialInverseKinematicsParameters(2, 2)
mut.DoDifferentialInverseKinematics(plant, context, np.zeros(6), frame,
parameters)
mut.DoDifferentialInverseKinematics(plant, context,
Isometry3.Identity(), frame,
parameters)
def CalcKinematics(self, X_L7E, l7_frame, world_frame, tree_iiwa,
context_iiwa, t_plan):
'''
:param X_L7E: transformation from frame E (end effector) to frame L7.
:param l7_frame: A BodyFrame object of frame L7.
:param world_frame: A BodyFrame object of the world frame.
:param tree_iiwa: A MultibodyTree object of the robot.
:param context_iiwa: A Context object that describes the current state of the robot.
:param t_plan: time passed since the beginning of this Plan, expressed in seconds.
:return: Jv_WL7q: geometric jacbian of point Q in frame L7.
p_HrQ: position of point Q relative to frame Hr.
'''
# calculate Geometric jacobian (6 by 7 matrix) of point Q in frame L7.
p_L7Q = X_L7E.multiply(p_EQ)
Jv_WL7q = tree_iiwa.CalcFrameGeometricJacobianExpressedInWorld(
context=context_iiwa, frame_B=l7_frame, p_BoFo_B=p_L7Q)
# Translation
# Hr: handle reference frame
# p_HrQ: position of point Q relative to frame Hr.
# X_WHr: transformation from Hr to world frame W.
X_WHr = Isometry3()
# Your code here
#------------------------------------------------------------------------#
angle = self.traj.value(t_plan)
X_WHr.set_rotation(
np.array([[np.cos(angle), np.sin(angle), 0.],
[-np.sin(angle), np.cos(angle), 0.], [0., 0., 1.]]))
X_WHr.set_translation(
p_WC_handle +
[-r_handle * np.sin(angle), -r_handle * np.cos(angle), 0.])
#------------------------------------------------------------------------#
X_HrW = X_WHr.inverse()
X_WL7 = tree_iiwa.CalcRelativeTransform(context_iiwa,
frame_A=world_frame,
frame_B=l7_frame)
p_WQ = X_WL7.multiply(p_L7Q)
p_HrQ = X_HrW.multiply(p_WQ)
return Jv_WL7q, p_HrQ
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)
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)
def CalcKinematics(self, X_L7E, l7_frame, world_frame, tree_iiwa,
context_iiwa, handle_angle_ref):
# calculate Geometric jacobian (6 by 7 matrix) of point Q in frame L7.
p_L7Q = X_L7E.multiply(p_EQ)
Jv_WL7q = tree_iiwa.CalcFrameGeometricJacobianExpressedInWorld(
context=context_iiwa, frame_B=l7_frame, p_BoFo_B=p_L7Q)
# Translation
# Hr: handle reference frame
# p_HrQ position of point Q relative to frame Hr.
# X_WHr: transformation from Hr to world frame W.
X_WHr = Isometry3()
X_HrW = X_WHr.inverse()
X_WL7 = tree_iiwa.CalcRelativeTransform(context_iiwa,
frame_A=world_frame,
frame_B=l7_frame)
p_WQ = X_WL7.multiply(p_L7Q)
p_HrQ = X_HrW.multiply(p_WQ)
return Jv_WL7q, p_HrQ
def read_poses_from_file(filename):
pose_dict = {}
row_num = 0
cur_matrix = np.eye(4)
with open(filename, "r") as f:
for line in f:
line = line.rstrip()
if not line.lstrip(" ").startswith("["):
object_name = line
else:
row = np.matrix(line)
cur_matrix[row_num, :] = row
row_num += 1
if row_num == 4:
pose_dict[object_name] = Isometry3(cur_matrix)
translation = pose_dict[object_name].translation()
translation[2] = -dz_table_top_robot_base
pose_dict[object_name].set_translation(translation)
pose_dict[object_name].set_rotation(np.eye(3))
cur_matrix = np.eye(4)
row_num %= 4
return pose_dict
def __init__(self,
time_step,
object_file_path,
object_base_link_name,
is_hardware=False):
self.object_base_link_name = object_base_link_name
self.time_step = time_step
self.is_hardware = is_hardware
# Finalize manipulation station by adding manipuland.
self.station = ManipulationStation(self.time_step)
self.station.AddCupboard()
self.plant = self.station.get_mutable_multibody_plant()
self.object = AddModelFromSdfFile(
file_name=object_file_path,
model_name="object",
plant=self.station.get_mutable_multibody_plant(),
scene_graph=self.station.get_mutable_scene_graph())
self.station.Finalize()
# Initial pose of the object
X_WObject = Isometry3.Identity()
X_WObject.set_translation([.6, 0, 0])
self.X_WObject = X_WObject
def test_model_instance_state_access(self):
# Create a multibodyplant with a kuka arm and a schunk gripper.
# the arm is welded to the world, the gripper is welded to the
# arm's end effector.
wsg50_sdf_path = FindResourceOrThrow(
"drake/manipulation/models/" +
"wsg_50_description/sdf/schunk_wsg_50.sdf")
iiwa_sdf_path = FindResourceOrThrow(
"drake/manipulation/models/" +
"iiwa_description/sdf/iiwa14_no_collision.sdf")
timestep = 0.0002
plant = MultibodyPlant(timestep)
iiwa_model = AddModelFromSdfFile(file_name=iiwa_sdf_path,
model_name='robot',
scene_graph=None,
plant=plant)
gripper_model = AddModelFromSdfFile(file_name=wsg50_sdf_path,
model_name='gripper',
scene_graph=None,
plant=plant)
# Weld the base of arm and gripper to reduce the number of states.
X_EeGripper = Isometry3.Identity()
X_EeGripper.set_translation([0, 0, 0.081])
X_EeGripper.set_rotation(
RollPitchYaw(np.pi / 2, 0, np.pi / 2).ToRotationMatrix().matrix())
plant.WeldFrames(A=plant.world_frame(),
B=plant.GetFrameByName("iiwa_link_0", iiwa_model))
plant.WeldFrames(A=plant.GetFrameByName("iiwa_link_7", iiwa_model),
B=plant.GetFrameByName("body", gripper_model),
X_AB=X_EeGripper)
plant.Finalize()
# Create a context of the MBP and set the state of the context
# to desired values.
context = plant.CreateDefaultContext()
tree = plant.tree()
nq = plant.num_positions()
nv = plant.num_velocities()
q_iiwa_desired = np.zeros(7)
v_iiwa_desired = np.zeros(7)
q_gripper_desired = np.zeros(2)
v_gripper_desired = np.zeros(2)
q_iiwa_desired[2] = np.pi / 3
q_gripper_desired[0] = 0.1
v_iiwa_desired[1] = 5.0
q_gripper_desired[0] = -0.3
x_plant_desired = np.zeros(nq + nv)
x_plant_desired[0:7] = q_iiwa_desired
x_plant_desired[7:9] = q_gripper_desired
x_plant_desired[nq:nq + 7] = v_iiwa_desired
x_plant_desired[nq + 7:nq + nv] = v_gripper_desired
x_plant = tree.get_mutable_multibody_state_vector(context)
x_plant[:] = x_plant_desired
# Get state from context.
x = tree.get_multibody_state_vector(context)
q = x[0:nq]
v = x[nq:nq + nv]
# Get positions and velocities of specific model instances
# from the postion/velocity vector of the plant.
q_iiwa = tree.get_positions_from_array(iiwa_model, q)
q_gripper = tree.get_positions_from_array(gripper_model, q)
v_iiwa = tree.get_velocities_from_array(iiwa_model, v)
v_gripper = tree.get_velocities_from_array(gripper_model, v)
# Assert that the get_positions_from_array return
# the desired values set earlier.
self.assertTrue(np.allclose(q_iiwa_desired, q_iiwa))
self.assertTrue(np.allclose(v_iiwa_desired, v_iiwa))
self.assertTrue(np.allclose(q_gripper_desired, q_gripper))
self.assertTrue(np.allclose(v_gripper_desired, v_gripper))
station_context.FixInputPort(
station.GetInputPort("iiwa_feedforward_torque").get_index(), np.zeros(7))
if not args.hardware:
# Set the initial positions of the IIWA to a comfortable configuration
# inside the workspace of the station.
q0 = [0, 0.6, 0, -1.75, 0, 1.0, 0]
station.SetIiwaPosition(q0, station_context)
station.SetIiwaVelocity(np.zeros(7), station_context)
# Set the initial configuration of the gripper to open.
station.SetWsgPosition(0.1, station_context)
station.SetWsgVelocity(0, station_context)
# Place the object in the middle of the workspace.
X_WObject = Isometry3.Identity()
X_WObject.set_translation([.6, 0, 0])
station.get_multibody_plant().tree().SetFreeBodyPoseOrThrow(
station.get_multibody_plant().GetBodyByName("base_link", object),
X_WObject,
station.GetMutableSubsystemContext(station.get_multibody_plant(),
station_context))
# Eval the output port once to read the initial positions of the IIWA.
q0 = station.GetOutputPort("iiwa_position_measured").Eval(
station_context).get_value()
teleop.set_position(q0)
# This is important to avoid duplicate publishes to the hardware interface:
simulator.set_publish_every_time_step(False)
def RunSimulation(self,
plans_list,
gripper_setpoint_list,
extra_time=0,
real_time_rate=1.0,
q0_kuka=np.zeros(7)):
'''
Constructs a Diagram that sends commands to ManipulationStation.
:param plans_list:
:param gripper_setpoint_list:
:param extra_time:
:param real_time_rate:
:param q0_kuka:
:return:
'''
builder = DiagramBuilder()
builder.AddSystem(self.station)
# Add plan runner
plan_runner = KukaPlanRunner(self.plant)
builder.AddSystem(plan_runner)
builder.Connect(plan_runner.get_output_port(0),
self.station.GetInputPort("iiwa_position"))
# Add state machine.
state_machine = ManipStateMachine(
plant=self.plant,
kuka_plans=plans_list,
gripper_setpoint_list=gripper_setpoint_list)
builder.AddSystem(state_machine)
builder.Connect(state_machine.kuka_plan_output_port,
plan_runner.plan_input_port)
builder.Connect(state_machine.hand_setpoint_output_port,
self.station.GetInputPort("wsg_position"))
builder.Connect(state_machine.gripper_force_limit_output_port,
self.station.GetInputPort("wsg_force_limit"))
builder.Connect(self.station.GetOutputPort("iiwa_position_measured"),
state_machine.iiwa_position_input_port)
# Add meshcat visualizer
from underactuated.meshcat_visualizer import MeshcatVisualizer
scene_graph = self.station.get_mutable_scene_graph()
viz = MeshcatVisualizer(scene_graph)
builder.AddSystem(viz)
builder.Connect(self.station.GetOutputPort("pose_bundle"),
viz.get_input_port(0))
# Add logger
iiwa_position_command_log = builder.AddSystem(
SignalLogger(self.station.GetInputPort("iiwa_position").size()))
iiwa_position_command_log._DeclarePeriodicPublish(0.005)
builder.Connect(plan_runner.get_output_port(0),
iiwa_position_command_log.get_input_port(0))
# build diagram
diagram = builder.Build()
viz.load()
time.sleep(2.0)
RenderSystemWithGraphviz(diagram)
# construct simulator
simulator = Simulator(diagram)
context = diagram.GetMutableSubsystemContext(
self.station, simulator.get_mutable_context())
# set initial state of the robot
self.station.SetIiwaPosition(q0_kuka, context)
self.station.SetIiwaVelocity(np.zeros(7), context)
self.station.SetWsgState(0.05, 0, context)
# set initial hinge angles of the cupboard.
left_hinge_joint = self.plant.GetJointByName("left_door_hinge")
left_hinge_joint.set_angle(
context=self.station.GetMutableSubsystemContext(
self.plant, context),
angle=-np.pi / 2)
right_hinge_joint = self.plant.GetJointByName("right_door_hinge")
right_hinge_joint.set_angle(
context=self.station.GetMutableSubsystemContext(
self.plant, context),
angle=np.pi / 2)
# set initial pose of the object
X_WObject = Isometry3.Identity()
X_WObject.set_translation([.6, 0, 0])
self.plant.tree().SetFreeBodyPoseOrThrow(
self.plant.GetBodyByName(self.object_base_link_name, self.object),
X_WObject,
self.station.GetMutableSubsystemContext(self.plant, context))
# fix feedforward torque input to 0.
context.FixInputPort(
self.station.GetInputPort("iiwa_feedforward_torque").get_index(),
np.zeros(7))
simulator.set_publish_every_time_step(False)
simulator.set_target_realtime_rate(real_time_rate)
simulator.Initialize()
sim_duration = 0.
for plan in plans_list:
sim_duration += plan.get_duration() * 1.1
sim_duration += extra_time
print "simulation duration", sim_duration
simulator.StepTo(sim_duration)
return iiwa_position_command_log
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.
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_pose(0, Isometry3(q3, p3))
bundle_value = AbstractValue.Make(bundle)
context.FixInputPort(0, pose1)
context.FixInputPort(1, pose2)
context.FixInputPort(2, velocity)
context.FixInputPort(3, bundle_value)
aggregator.CalcOutput(context, output)
value = output.get_data(0).get_value()
self.assertEqual(value.get_num_poses(), 3)
isom1_actual = Isometry3()
isom1_actual.set_translation(p1)
self.assertTrue(
(value.get_pose(0).matrix() == isom1_actual.matrix()).all())
isom2_actual = Isometry3()
isom2_actual.set_translation(p2)
self.assertTrue(
(value.get_pose(1).matrix() == isom2_actual.matrix()).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))
self.assertTrue(
(value.get_pose(2).matrix() == Isometry3(q3, p3).matrix()).all())
def test_multibody_tree_kinematics(self):
file_name = FindResourceOrThrow(
"drake/examples/double_pendulum/models/double_pendulum.sdf")
plant = MultibodyPlant()
AddModelFromSdfFile(file_name, plant)
plant.Finalize()
context = plant.CreateDefaultContext()
tree = plant.tree()
world_frame = plant.world_frame()
base = plant.GetBodyByName("base")
base_frame = plant.GetFrameByName("base")
X_WL = tree.CalcRelativeTransform(context,
frame_A=world_frame,
frame_B=base_frame)
self.assertIsInstance(X_WL, Isometry3)
p_AQi = tree.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 = tree.CalcFrameGeometricJacobianExpressedInWorld(
context=context, frame_B=base_frame, p_BoFo_B=[0, 0, 0])
self.assertTupleEqual(Jv_WL.shape, (6, plant.num_velocities()))
# Compute body pose.
X_WBase = tree.EvalBodyPoseInWorld(context, base)
self.assertIsInstance(X_WBase, Isometry3)
# All body poses.
X_WB_list = tree.CalcAllBodyPosesInWorld(context)
self.assertEqual(len(X_WB_list), 4)
for X_WB in X_WB_list:
self.assertIsInstance(X_WB, Isometry3)
# Compute body velocities.
v_WB_list = tree.CalcAllBodySpatialVelocitiesInWorld(context)
self.assertEqual(len(v_WB_list), 4)
for v_WB in v_WB_list:
self.assertIsInstance(v_WB, SpatialVelocity)
# - Sanity check.
v_WBase_flat = np.hstack(
(v_WB_list[0].rotational(), v_WB_list[0].translational()))
self.assertTrue(np.allclose(v_WBase_flat, np.zeros(6)))
# Set pose for the base.
X_WB_desired = Isometry3.Identity()
X_WB = tree.CalcRelativeTransform(context, world_frame, base_frame)
tree.SetFreeBodyPoseOrThrow(body=base,
X_WB=X_WB_desired,
context=context)
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])
tree.SetFreeBodySpatialVelocityOrThrow(body=base,
V_WB=v_WB,
context=context)
v_base = tree.EvalBodySpatialVelocityInWorld(context, base)
self.assertTrue(np.allclose(v_base.rotational(), v_WB.rotational()))
self.assertTrue(
np.allclose(v_base.translational(), v_WB.translational()))
def test_model_instance_state_access_by_array(self):
# Create a MultibodyPlant with a kuka arm and a schunk gripper.
# the arm is welded to the world, the gripper is welded to the
# arm's end effector.
wsg50_sdf_path = FindResourceOrThrow(
"drake/manipulation/models/" +
"wsg_50_description/sdf/schunk_wsg_50.sdf")
iiwa_sdf_path = FindResourceOrThrow(
"drake/manipulation/models/" +
"iiwa_description/sdf/iiwa14_no_collision.sdf")
timestep = 0.0002
plant = MultibodyPlant(timestep)
parser = Parser(plant)
iiwa_model = parser.AddModelFromFile(file_name=iiwa_sdf_path,
model_name='robot')
gripper_model = parser.AddModelFromFile(file_name=wsg50_sdf_path,
model_name='gripper')
# Weld the base of arm and gripper to reduce the number of states.
X_EeGripper = Isometry3.Identity()
X_EeGripper.set_translation([0, 0, 0.081])
X_EeGripper.set_rotation(
RollPitchYaw(np.pi / 2, 0, np.pi / 2).ToRotationMatrix().matrix())
plant.WeldFrames(A=plant.world_frame(),
B=plant.GetFrameByName("iiwa_link_0", iiwa_model))
plant.WeldFrames(A=plant.GetFrameByName("iiwa_link_7", iiwa_model),
B=plant.GetFrameByName("body", gripper_model),
X_AB=X_EeGripper)
plant.Finalize()
# Create a context of the MBP and set the state of the context
# to desired values.
context = plant.CreateDefaultContext()
nq = plant.num_positions()
nq_iiwa = plant.num_positions(iiwa_model)
nv = plant.num_velocities()
nv_iiwa = plant.num_velocities(iiwa_model)
q_iiwa_desired = np.linspace(0, 0.3, 7)
v_iiwa_desired = q_iiwa_desired + 0.4
q_gripper_desired = [0.4, 0.5]
v_gripper_desired = [-1., -2.]
x_desired = np.zeros(nq + nv)
x_desired[0:7] = q_iiwa_desired
x_desired[7:9] = q_gripper_desired
x_desired[nq:nq + 7] = v_iiwa_desired
x_desired[nq + 7:nq + nv] = v_gripper_desired
x = plant.GetMutablePositionsAndVelocities(context=context)
x[:] = x_desired
q = plant.GetPositions(context=context)
v = plant.GetVelocities(context=context)
# Get state from context.
x = plant.GetPositionsAndVelocities(context=context)
x_tmp = plant.GetMutablePositionsAndVelocities(context=context)
self.assertTrue(np.allclose(x_desired, x_tmp))
# Get positions and velocities of specific model instances
# from the position/velocity vector of the plant.
q_iiwa = plant.GetPositions(context=context, model_instance=iiwa_model)
q_iiwa_array = plant.GetPositionsFromArray(model_instance=iiwa_model,
q=q)
self.assertTrue(np.allclose(q_iiwa, q_iiwa_array))
q_gripper = plant.GetPositions(context=context,
model_instance=gripper_model)
v_iiwa = plant.GetVelocities(context=context,
model_instance=iiwa_model)
v_iiwa_array = plant.GetVelocitiesFromArray(model_instance=iiwa_model,
v=v)
self.assertTrue(np.allclose(v_iiwa, v_iiwa_array))
v_gripper = plant.GetVelocities(context=context,
model_instance=gripper_model)
# Assert that the `GetPositions` and `GetVelocities` return
# the desired values set earlier.
self.assertTrue(np.allclose(q_iiwa_desired, q_iiwa))
self.assertTrue(np.allclose(v_iiwa_desired, v_iiwa))
self.assertTrue(np.allclose(q_gripper_desired, q_gripper))
self.assertTrue(np.allclose(v_gripper_desired, v_gripper))
# Verify that SetPositionsInArray() and SetVelocitiesInArray() works.
plant.SetPositionsInArray(model_instance=iiwa_model,
q_instance=np.zeros(nq_iiwa),
q=q)
self.assertTrue(
np.allclose(
plant.GetPositionsFromArray(model_instance=iiwa_model, q=q),
np.zeros(nq_iiwa)))
plant.SetVelocitiesInArray(model_instance=iiwa_model,
v_instance=np.zeros(nv_iiwa),
v=v)
self.assertTrue(
np.allclose(
plant.GetVelocitiesFromArray(model_instance=iiwa_model, v=v),
np.zeros(nv_iiwa)))
# Check actuation.
nu = plant.num_actuated_dofs()
u = np.zeros(nu)
u_iiwa = np.arange(nv_iiwa)
plant.SetActuationInArray(model_instance=iiwa_model,
u_instance=u_iiwa,
u=u)
self.assertTrue(np.allclose(u[:7], u_iiwa))
def test_model_instance_state_access(self):
# Create a MultibodyPlant with a kuka arm and a schunk gripper.
# the arm is welded to the world, the gripper is welded to the
# arm's end effector.
wsg50_sdf_path = FindResourceOrThrow(
"drake/manipulation/models/" +
"wsg_50_description/sdf/schunk_wsg_50.sdf")
iiwa_sdf_path = FindResourceOrThrow(
"drake/manipulation/models/" +
"iiwa_description/sdf/iiwa14_no_collision.sdf")
plant = MultibodyPlant()
parser = Parser(plant)
iiwa_model = parser.AddModelFromFile(file_name=iiwa_sdf_path,
model_name='robot')
gripper_model = parser.AddModelFromFile(file_name=wsg50_sdf_path,
model_name='gripper')
# Weld the base of arm and gripper to reduce the number of states.
X_EeGripper = Isometry3.Identity()
X_EeGripper.set_translation([0, 0, 0.081])
X_EeGripper.set_rotation(
RollPitchYaw(np.pi / 2, 0, np.pi / 2).ToRotationMatrix().matrix())
plant.WeldFrames(A=plant.world_frame(),
B=plant.GetFrameByName("iiwa_link_0", iiwa_model))
plant.WeldFrames(A=plant.GetFrameByName("iiwa_link_7", iiwa_model),
B=plant.GetFrameByName("body", gripper_model),
X_AB=X_EeGripper)
plant.Finalize()
# Create a context of the MBP and set the state of the context
# to desired values.
context = plant.CreateDefaultContext()
nq = plant.num_positions()
nv = plant.num_velocities()
nq_iiwa = 7
nv_iiwa = 7
nq_gripper = 2
nv_gripper = 2
q_iiwa_desired = np.zeros(nq_iiwa)
v_iiwa_desired = np.zeros(nv_iiwa)
q_gripper_desired = np.zeros(nq_gripper)
v_gripper_desired = np.zeros(nv_gripper)
q_iiwa_desired[2] = np.pi / 3
q_gripper_desired[0] = 0.1
v_iiwa_desired[1] = 5.0
q_gripper_desired[0] = -0.3
x_iiwa_desired = np.zeros(nq_iiwa + nv_iiwa)
x_iiwa_desired[0:nq_iiwa] = q_iiwa_desired
x_iiwa_desired[nq_iiwa:nq_iiwa + nv_iiwa] = v_iiwa_desired
x_gripper_desired = np.zeros(nq_gripper + nv_gripper)
x_gripper_desired[0:nq_gripper] = q_gripper_desired
x_gripper_desired[nq_gripper:nq_gripper +
nv_gripper] = v_gripper_desired
x_desired = np.zeros(nq + nv)
x_desired[0:7] = q_iiwa_desired
x_desired[7:9] = q_gripper_desired
x_desired[nq:nq + 7] = v_iiwa_desired
x_desired[nq + 7:nq + nv] = v_gripper_desired
# Check SetPositionsAndVelocities() for each model instance.
# Do the iiwa model first.
plant.SetPositionsAndVelocities(context, np.zeros(nq + nv))
self.assertTrue(
np.allclose(plant.GetPositionsAndVelocities(context),
np.zeros(nq + nv)))
plant.SetPositionsAndVelocities(context, iiwa_model, x_iiwa_desired)
self.assertTrue(
np.allclose(plant.GetPositionsAndVelocities(context, iiwa_model),
x_iiwa_desired))
self.assertTrue(
np.allclose(
plant.GetPositionsAndVelocities(context, gripper_model),
np.zeros(nq_gripper + nv_gripper)))
# Do the gripper model.
plant.SetPositionsAndVelocities(context, np.zeros(nq + nv))
self.assertTrue(
np.allclose(plant.GetPositionsAndVelocities(context),
np.zeros(nq + nv)))
plant.SetPositionsAndVelocities(context, gripper_model,
x_gripper_desired)
self.assertTrue(
np.allclose(
plant.GetPositionsAndVelocities(context, gripper_model),
x_gripper_desired))
self.assertTrue(
np.allclose(plant.GetPositionsAndVelocities(context, iiwa_model),
np.zeros(nq_iiwa + nv_iiwa)))
# Check SetPositions() for each model instance.
# Do the iiwa model first.
plant.SetPositionsAndVelocities(context, np.zeros(nq + nv))
self.assertTrue(
np.allclose(plant.GetPositionsAndVelocities(context),
np.zeros(nq + nv)))
plant.SetPositions(context, iiwa_model, q_iiwa_desired)
self.assertTrue(
np.allclose(plant.GetPositions(context, iiwa_model),
q_iiwa_desired))
self.assertTrue(
np.allclose(plant.GetVelocities(context, iiwa_model),
np.zeros(nv_iiwa)))
self.assertTrue(
np.allclose(
plant.GetPositionsAndVelocities(context, gripper_model),
np.zeros(nq_gripper + nv_gripper)))
# Do the gripper model.
plant.SetPositionsAndVelocities(context, np.zeros(nq + nv))
self.assertTrue(
np.allclose(plant.GetPositionsAndVelocities(context),
np.zeros(nq + nv)))
plant.SetPositions(context, gripper_model, q_gripper_desired)
self.assertTrue(
np.allclose(plant.GetPositions(context, gripper_model),
q_gripper_desired))
self.assertTrue(
np.allclose(plant.GetVelocities(context, gripper_model),
np.zeros(nq_gripper)))
self.assertTrue(
np.allclose(plant.GetPositionsAndVelocities(context, iiwa_model),
np.zeros(nq_iiwa + nv_iiwa)))
# Check SetVelocities() for each model instance.
# Do the iiwa model first.
plant.SetPositionsAndVelocities(context, np.zeros(nq + nv))
self.assertTrue(
np.allclose(plant.GetPositionsAndVelocities(context),
np.zeros(nq + nv)))
plant.SetVelocities(context, iiwa_model, v_iiwa_desired)
self.assertTrue(
np.allclose(plant.GetVelocities(context, iiwa_model),
v_iiwa_desired))
self.assertTrue(
np.allclose(plant.GetPositions(context, iiwa_model),
np.zeros(nq_iiwa)))
self.assertTrue(
np.allclose(
plant.GetPositionsAndVelocities(context, gripper_model),
np.zeros(nq_gripper + nv_gripper)))
# Do the gripper model.
plant.SetPositionsAndVelocities(context, np.zeros(nq + nv))
self.assertTrue(
np.allclose(plant.GetPositionsAndVelocities(context),
np.zeros(nq + nv)))
plant.SetVelocities(context, gripper_model, v_gripper_desired)
self.assertTrue(
np.allclose(plant.GetVelocities(context, gripper_model),
v_gripper_desired))
self.assertTrue(
np.allclose(plant.GetPositions(context, gripper_model),
np.zeros(nv_gripper)))
self.assertTrue(
np.allclose(plant.GetPositionsAndVelocities(context, iiwa_model),
np.zeros(nq_iiwa + nv_iiwa)))
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, Isometry3)
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, Isometry3)
# Set pose for the base.
X_WB_desired = Isometry3.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())
