def BuildHand(num_fingers=3, manipuland_sdf="models/manipuland_box.sdf"):
''' Build up the hand by replicating a finger
model at a handful of base positions. '''
finger_base_positions = np.vstack([
np.abs(np.linspace(-0.25, 0.25, num_fingers)),
np.linspace(0.25, -0.25, num_fingers),
np.zeros(num_fingers)
]).T
tree = RigidBodyTree()
for i, base_pos in enumerate(finger_base_positions):
frame = RigidBodyFrame(name="finger_%d_base_frame" % i,
body=tree.world(),
xyz=base_pos,
rpy=[0, 0, 0])
tree.addFrame(frame)
AddModelInstancesFromSdfString(
open("models/planar_finger.sdf", 'r').read(),
FloatingBaseType.kFixed, frame, tree)
# Add the manipuland as well from an sdf.
manipuland_frame = RigidBodyFrame(name="manipuland_frame",
body=tree.world(),
xyz=[0, 0., 0.],
rpy=[0, 0, 0])
tree.addFrame(manipuland_frame)
AddModelInstancesFromSdfString(
open(manipuland_sdf, 'r').read(), FloatingBaseType.kFixed,
manipuland_frame, tree)
return tree
hand_controller.setpoint_input_port)
builder.Connect(hand_controller.get_output_port(0),
rbplant_sys.get_input_port(1))
# Hook up the visualizer we created earlier.
visualizer = builder.AddSystem(pbrv)
builder.Connect(rbplant_sys.state_output_port(),
visualizer.get_input_port(0))
# Add a camera, too, though no controller or estimator
# will consume the output of it.
# - Add frame for camera fixture.
camera_frame = RigidBodyFrame(
name="rgbd camera frame", body=rbt.world(),
xyz=[2, 0., 1.5], rpy=[-np.pi/4, 0., -np.pi])
rbt.addFrame(camera_frame)
camera = builder.AddSystem(
RgbdCamera(name="camera", tree=rbt, frame=camera_frame,
z_near=0.5, z_far=2.0, fov_y=np.pi / 4,
width=320, height=240,
show_window=False))
builder.Connect(rbplant_sys.state_output_port(),
camera.get_input_port(0))
camera_meshcat_visualizer = builder.AddSystem(
kuka_utils.RgbdCameraMeshcatVisualizer(camera, rbt))
builder.Connect(camera.depth_image_output_port(),
camera_meshcat_visualizer.camera_input_port)
builder.Connect(rbplant_sys.state_output_port(),
camera_meshcat_visualizer.state_input_port)