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)