def GetHomeConfiguration(is_printing=True): """ Returns a configuration of the MultibodyPlant in which point Q (defined by global variable p_EQ) in robot EE frame is at p_WQ_home, and orientation of frame Ea is R_WEa_ref. """ # get "home" pose ik_scene = inverse_kinematics.InverseKinematics(plant) theta_bound = 0.005 * np.pi # 0.9 degrees X_EEa = GetEndEffectorWorldAlignedFrame() R_EEa = RotationMatrix(X_EEa.rotation()) ik_scene.AddOrientationConstraint(frameAbar=world_frame, R_AbarA=R_WL7_ref, frameBbar=l7_frame, R_BbarB=RotationMatrix.Identity(), theta_bound=theta_bound) p_WQ0 = p_WQ_home p_WQ_lower = p_WQ0 - 0.005 p_WQ_upper = p_WQ0 + 0.005 ik_scene.AddPositionConstraint(frameB=l7_frame, p_BQ=p_L7Q, frameA=world_frame, p_AQ_lower=p_WQ_lower, p_AQ_upper=p_WQ_upper) prog = ik_scene.prog() prog.SetInitialGuess(ik_scene.q(), np.zeros(plant.num_positions())) result = prog.Solve() if is_printing: print result return prog.GetSolution(ik_scene.q())
def SolveOneShotIk( p_WQ_ref, R_WL7_ref, p_L7Q, q_initial_guess, position_tolerance=0.005, theta_bound=0.005): ik = inverse_kinematics.InverseKinematics(plant) ik.AddOrientationConstraint( frameAbar=world_frame, R_AbarA=R_WL7_ref, frameBbar=l7_frame, R_BbarB=RotationMatrix.Identity(), theta_bound=theta_bound) p_WQ_lower = p_WQ_ref - position_tolerance p_WQ_upper = p_WQ_ref + position_tolerance ik.AddPositionConstraint( frameB=l7_frame, p_BQ=p_L7Q, frameA=world_frame, p_AQ_lower=p_WQ_lower, p_AQ_upper=p_WQ_upper) prog = ik.prog() prog.SetInitialGuess(ik.q(), q_initial_guess) result = mp.Solve(prog) if result.get_solution_result() != SolutionResult.kSolutionFound: print(result.get_solution_result()) raise RuntimeError return result.GetSolution(ik.q())
def solve_inverse_kinematics(mbp, target_frame, target_pose, max_position_error=0.005, theta_bound=0.01*np.pi, initial_guess=None): if initial_guess is None: initial_guess = np.zeros(mbp.num_positions()) for joint in prune_fixed_joints(get_joints(mbp)): lower, upper = get_joint_limits(joint) if -np.inf < lower < upper < np.inf: initial_guess[joint.position_start()] = random.uniform(lower, upper) assert mbp.num_positions() == len(initial_guess) ik_scene = InverseKinematics(mbp) world_frame = mbp.world_frame() ik_scene.AddOrientationConstraint( frameAbar=target_frame, R_AbarA=RotationMatrix.Identity(), frameBbar=world_frame, R_BbarB=RotationMatrix(target_pose.rotation()), theta_bound=theta_bound) lower = target_pose.translation() - max_position_error upper = target_pose.translation() + max_position_error ik_scene.AddPositionConstraint( frameB=target_frame, p_BQ=np.zeros(3), frameA=world_frame, p_AQ_lower=lower, p_AQ_upper=upper) prog = ik_scene.prog() prog.SetInitialGuess(ik_scene.q(), initial_guess) result = prog.Solve() if result != SolutionResult.kSolutionFound: return None return prog.GetSolution(ik_scene.q())
def main(): parser = argparse.ArgumentParser(description=__doc__) parser.add_argument( "--target_realtime_rate", type=float, default=1.0, help="Desired rate relative to real time. See documentation for " "Simulator::set_target_realtime_rate() for details.") parser.add_argument("--duration", type=float, default=np.inf, help="Desired duration of the simulation in seconds.") parser.add_argument( "--hardware", action='store_true', help="Use the ManipulationStationHardwareInterface instead of an " "in-process simulation.") parser.add_argument("--test", action='store_true', help="Disable opening the gui window for testing.") parser.add_argument( '--setup', type=str, default='manipulation_class', help="The manipulation station setup to simulate. ", choices=['manipulation_class', 'clutter_clearing', 'planar']) parser.add_argument( "-w", "--open-window", dest="browser_new", action="store_const", const=1, default=None, help="Open the MeshCat display in a new browser window.") args = parser.parse_args() builder = DiagramBuilder() # NOTE: the meshcat instance is always created in order to create the # teleop controls (joint sliders and open/close gripper button). When # args.hardware is True, the meshcat server will *not* display robot # geometry, but it will contain the joint sliders and open/close gripper # button in the "Open Controls" tab in the top-right of the viewing server. meshcat = Meshcat() if args.hardware: # TODO(russt): Replace this hard-coded camera serial number with a # config file. camera_ids = ["805212060544"] station = builder.AddSystem( ManipulationStationHardwareInterface(camera_ids)) station.Connect(wait_for_cameras=False) else: station = builder.AddSystem(ManipulationStation()) # Initializes the chosen station type. if args.setup == 'manipulation_class': station.SetupManipulationClassStation() station.AddManipulandFromFile( "drake/examples/manipulation_station/models/" + "061_foam_brick.sdf", RigidTransform(RotationMatrix.Identity(), [0.6, 0, 0])) elif args.setup == 'clutter_clearing': station.SetupClutterClearingStation() ycb_objects = CreateClutterClearingYcbObjectList() for model_file, X_WObject in ycb_objects: station.AddManipulandFromFile(model_file, X_WObject) elif args.setup == 'planar': station.SetupPlanarIiwaStation() station.AddManipulandFromFile( "drake/examples/manipulation_station/models/" + "061_foam_brick.sdf", RigidTransform(RotationMatrix.Identity(), [0.6, 0, 0])) station.Finalize() geometry_query_port = station.GetOutputPort("geometry_query") DrakeVisualizer.AddToBuilder(builder, geometry_query_port) meshcat_visualizer = MeshcatVisualizerCpp.AddToBuilder( builder=builder, query_object_port=geometry_query_port, meshcat=meshcat) if args.setup == 'planar': meshcat.Set2dRenderMode() pyplot_visualizer = ConnectPlanarSceneGraphVisualizer( builder, station.get_scene_graph(), geometry_query_port) if args.browser_new is not None: url = meshcat.web_url() webbrowser.open(url=url, new=args.browser_new) teleop = builder.AddSystem( JointSliders(meshcat=meshcat, plant=station.get_controller_plant())) num_iiwa_joints = station.num_iiwa_joints() filter = builder.AddSystem( FirstOrderLowPassFilter(time_constant=2.0, size=num_iiwa_joints)) builder.Connect(teleop.get_output_port(0), filter.get_input_port(0)) builder.Connect(filter.get_output_port(0), station.GetInputPort("iiwa_position")) wsg_buttons = builder.AddSystem(SchunkWsgButtons(meshcat=meshcat)) builder.Connect(wsg_buttons.GetOutputPort("position"), station.GetInputPort("wsg_position")) builder.Connect(wsg_buttons.GetOutputPort("force_limit"), station.GetInputPort("wsg_force_limit")) # When in regression test mode, log our joint velocities to later check # that they were sufficiently quiet. if args.test: iiwa_velocities = builder.AddSystem(VectorLogSink(num_iiwa_joints)) builder.Connect(station.GetOutputPort("iiwa_velocity_estimated"), iiwa_velocities.get_input_port(0)) else: iiwa_velocities = None diagram = builder.Build() simulator = Simulator(diagram) # This is important to avoid duplicate publishes to the hardware interface: simulator.set_publish_every_time_step(False) station_context = diagram.GetMutableSubsystemContext( station, simulator.get_mutable_context()) station.GetInputPort("iiwa_feedforward_torque").FixValue( station_context, np.zeros(num_iiwa_joints)) # If the diagram is only the hardware interface, then we must advance it a # little bit so that first LCM messages get processed. A simulated plant is # already publishing correct positions even without advancing, and indeed # we must not advance a simulated plant until the sliders and filters have # been initialized to match the plant. if args.hardware: simulator.AdvanceTo(1e-6) # Eval the output port once to read the initial positions of the IIWA. q0 = station.GetOutputPort("iiwa_position_measured").Eval(station_context) teleop.SetPositions(q0) filter.set_initial_output_value( diagram.GetMutableSubsystemContext(filter, simulator.get_mutable_context()), q0) simulator.set_target_realtime_rate(args.target_realtime_rate) simulator.AdvanceTo(args.duration) # Ensure that our initialization logic was correct, by inspecting our # logged joint velocities. if args.test: iiwa_velocities_log = iiwa_velocities.FindLog(simulator.get_context()) for time, qdot in zip(iiwa_velocities_log.sample_times(), iiwa_velocities_log.data().transpose()): # TODO(jwnimmer-tri) We should be able to do better than a 40 # rad/sec limit, but that's the best we can enforce for now. if qdot.max() > 0.1: print(f"ERROR: large qdot {qdot} at time {time}") sys.exit(1)
def main(): parser = argparse.ArgumentParser(description=__doc__) parser.add_argument( "--target_realtime_rate", type=float, default=1.0, help="Desired rate relative to real time. See documentation for " "Simulator::set_target_realtime_rate() for details.") parser.add_argument( "--duration", type=float, default=np.inf, help="Desired duration of the simulation in seconds.") parser.add_argument( "--hardware", action='store_true', help="Use the ManipulationStationHardwareInterface instead of an " "in-process simulation.") parser.add_argument( "--test", action='store_true', help="Disable opening the gui window for testing.") parser.add_argument( "--filter_time_const", type=float, default=0.1, help="Time constant for the first order low pass filter applied to" "the teleop commands") parser.add_argument( "--velocity_limit_factor", type=float, default=1.0, help="This value, typically between 0 and 1, further limits the " "iiwa14 joint velocities. It multiplies each of the seven " "pre-defined joint velocity limits. " "Note: The pre-defined velocity limits are specified by " "iiwa14_velocity_limits, found in this python file.") parser.add_argument( '--setup', type=str, default='manipulation_class', help="The manipulation station setup to simulate. ", choices=['manipulation_class', 'clutter_clearing', 'planar']) parser.add_argument( '--schunk_collision_model', type=str, default='box', help="The Schunk collision model to use for simulation. ", choices=['box', 'box_plus_fingertip_spheres']) MeshcatVisualizer.add_argparse_argument(parser) args = parser.parse_args() builder = DiagramBuilder() if args.hardware: station = builder.AddSystem(ManipulationStationHardwareInterface()) station.Connect(wait_for_cameras=False) else: station = builder.AddSystem(ManipulationStation()) if args.schunk_collision_model == "box": schunk_model = SchunkCollisionModel.kBox elif args.schunk_collision_model == "box_plus_fingertip_spheres": schunk_model = SchunkCollisionModel.kBoxPlusFingertipSpheres # Initializes the chosen station type. if args.setup == 'manipulation_class': station.SetupManipulationClassStation( schunk_model=schunk_model) station.AddManipulandFromFile( "drake/examples/manipulation_station/models/" + "061_foam_brick.sdf", RigidTransform(RotationMatrix.Identity(), [0.6, 0, 0])) elif args.setup == 'clutter_clearing': station.SetupClutterClearingStation( schunk_model=schunk_model) ycb_objects = CreateClutterClearingYcbObjectList() for model_file, X_WObject in ycb_objects: station.AddManipulandFromFile(model_file, X_WObject) elif args.setup == 'planar': station.SetupPlanarIiwaStation( schunk_model=schunk_model) station.AddManipulandFromFile( "drake/examples/manipulation_station/models/" + "061_foam_brick.sdf", RigidTransform(RotationMatrix.Identity(), [0.6, 0, 0])) station.Finalize() # If using meshcat, don't render the cameras, since RgbdCamera # rendering only works with drake-visualizer. Without this check, # running this code in a docker container produces libGL errors. if args.meshcat: meshcat = ConnectMeshcatVisualizer( builder, output_port=station.GetOutputPort("geometry_query"), zmq_url=args.meshcat, open_browser=args.open_browser) if args.setup == 'planar': meshcat.set_planar_viewpoint() elif args.setup == 'planar': pyplot_visualizer = builder.AddSystem(PlanarSceneGraphVisualizer( station.get_scene_graph())) builder.Connect(station.GetOutputPort("pose_bundle"), pyplot_visualizer.get_input_port(0)) else: DrakeVisualizer.AddToBuilder(builder, station.GetOutputPort("query_object")) image_to_lcm_image_array = builder.AddSystem( ImageToLcmImageArrayT()) image_to_lcm_image_array.set_name("converter") for name in station.get_camera_names(): cam_port = ( image_to_lcm_image_array .DeclareImageInputPort[PixelType.kRgba8U]( "camera_" + name)) builder.Connect( station.GetOutputPort("camera_" + name + "_rgb_image"), cam_port) image_array_lcm_publisher = builder.AddSystem( LcmPublisherSystem.Make( channel="DRAKE_RGBD_CAMERA_IMAGES", lcm_type=image_array_t, lcm=None, publish_period=0.1, use_cpp_serializer=True)) image_array_lcm_publisher.set_name("rgbd_publisher") builder.Connect( image_to_lcm_image_array.image_array_t_msg_output_port(), image_array_lcm_publisher.get_input_port(0)) robot = station.get_controller_plant() params = DifferentialInverseKinematicsParameters(robot.num_positions(), robot.num_velocities()) time_step = 0.005 params.set_timestep(time_step) # True velocity limits for the IIWA14 (in rad, rounded down to the first # decimal) iiwa14_velocity_limits = np.array([1.4, 1.4, 1.7, 1.3, 2.2, 2.3, 2.3]) if args.setup == 'planar': # Extract the 3 joints that are not welded in the planar version. iiwa14_velocity_limits = iiwa14_velocity_limits[1:6:2] # The below constant is in body frame. params.set_end_effector_velocity_gain([1, 0, 0, 0, 1, 1]) # Stay within a small fraction of those limits for this teleop demo. factor = args.velocity_limit_factor params.set_joint_velocity_limits((-factor*iiwa14_velocity_limits, factor*iiwa14_velocity_limits)) differential_ik = builder.AddSystem(DifferentialIK( robot, robot.GetFrameByName("iiwa_link_7"), params, time_step)) builder.Connect(differential_ik.GetOutputPort("joint_position_desired"), station.GetInputPort("iiwa_position")) teleop = builder.AddSystem(EndEffectorTeleop(args.setup == 'planar')) if args.test: teleop.window.withdraw() # Don't display the window when testing. filter = builder.AddSystem( FirstOrderLowPassFilter(time_constant=args.filter_time_const, size=6)) builder.Connect(teleop.get_output_port(0), filter.get_input_port(0)) builder.Connect(filter.get_output_port(0), differential_ik.GetInputPort("rpy_xyz_desired")) wsg_buttons = builder.AddSystem(SchunkWsgButtons(teleop.window)) builder.Connect(wsg_buttons.GetOutputPort("position"), station.GetInputPort("wsg_position")) builder.Connect(wsg_buttons.GetOutputPort("force_limit"), station.GetInputPort("wsg_force_limit")) # When in regression test mode, log our joint velocities to later check # that they were sufficiently quiet. num_iiwa_joints = station.num_iiwa_joints() if args.test: iiwa_velocities = builder.AddSystem(SignalLogger(num_iiwa_joints)) builder.Connect(station.GetOutputPort("iiwa_velocity_estimated"), iiwa_velocities.get_input_port(0)) else: iiwa_velocities = None diagram = builder.Build() simulator = Simulator(diagram) # This is important to avoid duplicate publishes to the hardware interface: simulator.set_publish_every_time_step(False) station_context = diagram.GetMutableSubsystemContext( station, simulator.get_mutable_context()) station.GetInputPort("iiwa_feedforward_torque").FixValue( station_context, np.zeros(num_iiwa_joints)) # If the diagram is only the hardware interface, then we must advance it a # little bit so that first LCM messages get processed. A simulated plant is # already publishing correct positions even without advancing, and indeed # we must not advance a simulated plant until the sliders and filters have # been initialized to match the plant. if args.hardware: simulator.AdvanceTo(1e-6) q0 = station.GetOutputPort("iiwa_position_measured").Eval( station_context) differential_ik.parameters.set_nominal_joint_position(q0) teleop.SetPose(differential_ik.ForwardKinematics(q0)) filter.set_initial_output_value( diagram.GetMutableSubsystemContext( filter, simulator.get_mutable_context()), teleop.get_output_port(0).Eval(diagram.GetMutableSubsystemContext( teleop, simulator.get_mutable_context()))) differential_ik.SetPositions(diagram.GetMutableSubsystemContext( differential_ik, simulator.get_mutable_context()), q0) simulator.set_target_realtime_rate(args.target_realtime_rate) simulator.AdvanceTo(args.duration) # Ensure that our initialization logic was correct, by inspecting our # logged joint velocities. if args.test: for time, qdot in zip(iiwa_velocities.sample_times(), iiwa_velocities.data().transpose()): # TODO(jwnimmer-tri) We should be able to do better than a 40 # rad/sec limit, but that's the best we can enforce for now. if qdot.max() > 0.1: print(f"ERROR: large qdot {qdot} at time {time}") sys.exit(1)
def main(): parser = argparse.ArgumentParser(description=__doc__) parser.add_argument( "--target_realtime_rate", type=float, default=1.0, help="Desired rate relative to real time. See documentation for " "Simulator::set_target_realtime_rate() for details.") parser.add_argument( "--duration", type=float, default=np.inf, help="Desired duration of the simulation in seconds.") parser.add_argument( "--hardware", action='store_true', help="Use the ManipulationStationHardwareInterface instead of an " "in-process simulation.") parser.add_argument( "--test", action='store_true', help="Disable opening the gui window for testing.") parser.add_argument( "--time_step", type=float, default=0.005, help="Time constant for the differential IK solver and first order" "low pass filter applied to the teleop commands") parser.add_argument( "--velocity_limit_factor", type=float, default=1.0, help="This value, typically between 0 and 1, further limits the " "iiwa14 joint velocities. It multiplies each of the seven " "pre-defined joint velocity limits. " "Note: The pre-defined velocity limits are specified by " "iiwa14_velocity_limits, found in this python file.") parser.add_argument( '--setup', type=str, default='manipulation_class', help="The manipulation station setup to simulate. ", choices=['manipulation_class', 'clutter_clearing']) parser.add_argument( '--schunk_collision_model', type=str, default='box', help="The Schunk collision model to use for simulation. ", choices=['box', 'box_plus_fingertip_spheres']) MeshcatVisualizer.add_argparse_argument(parser) args = parser.parse_args() if args.test: # Don't grab mouse focus during testing. # See: https://stackoverflow.com/a/52528832/7829525 os.environ["SDL_VIDEODRIVER"] = "dummy" builder = DiagramBuilder() if args.hardware: station = builder.AddSystem(ManipulationStationHardwareInterface()) station.Connect(wait_for_cameras=False) else: station = builder.AddSystem(ManipulationStation()) if args.schunk_collision_model == "box": schunk_model = SchunkCollisionModel.kBox elif args.schunk_collision_model == "box_plus_fingertip_spheres": schunk_model = SchunkCollisionModel.kBoxPlusFingertipSpheres # Initializes the chosen station type. if args.setup == 'manipulation_class': station.SetupManipulationClassStation( schunk_model=schunk_model) station.AddManipulandFromFile( ("drake/examples/manipulation_station/models/" "061_foam_brick.sdf"), RigidTransform(RotationMatrix.Identity(), [0.6, 0, 0])) elif args.setup == 'clutter_clearing': station.SetupClutterClearingStation( schunk_model=schunk_model) ycb_objects = CreateClutterClearingYcbObjectList() for model_file, X_WObject in ycb_objects: station.AddManipulandFromFile(model_file, X_WObject) station.Finalize() DrakeVisualizer.AddToBuilder(builder, station.GetOutputPort("query_object")) if args.meshcat: meshcat = ConnectMeshcatVisualizer( builder, output_port=station.GetOutputPort("geometry_query"), zmq_url=args.meshcat, open_browser=args.open_browser) if args.setup == 'planar': meshcat.set_planar_viewpoint() robot = station.get_controller_plant() params = DifferentialInverseKinematicsParameters(robot.num_positions(), robot.num_velocities()) params.set_timestep(args.time_step) # True velocity limits for the IIWA14 (in rad/s, rounded down to the first # decimal) iiwa14_velocity_limits = np.array([1.4, 1.4, 1.7, 1.3, 2.2, 2.3, 2.3]) # Stay within a small fraction of those limits for this teleop demo. factor = args.velocity_limit_factor params.set_joint_velocity_limits((-factor*iiwa14_velocity_limits, factor*iiwa14_velocity_limits)) differential_ik = builder.AddSystem(DifferentialIK( robot, robot.GetFrameByName("iiwa_link_7"), params, args.time_step)) builder.Connect(differential_ik.GetOutputPort("joint_position_desired"), station.GetInputPort("iiwa_position")) teleop = builder.AddSystem(DualShock4Teleop(initialize_joystick())) filter_ = builder.AddSystem( FirstOrderLowPassFilter(time_constant=args.time_step, size=6)) builder.Connect(teleop.get_output_port(0), filter_.get_input_port(0)) builder.Connect(filter_.get_output_port(0), differential_ik.GetInputPort("rpy_xyz_desired")) builder.Connect(teleop.GetOutputPort("position"), station.GetInputPort( "wsg_position")) builder.Connect(teleop.GetOutputPort("force_limit"), station.GetInputPort("wsg_force_limit")) diagram = builder.Build() simulator = Simulator(diagram) # This is important to avoid duplicate publishes to the hardware interface: simulator.set_publish_every_time_step(False) station_context = diagram.GetMutableSubsystemContext( station, simulator.get_mutable_context()) station.GetInputPort("iiwa_feedforward_torque").FixValue( station_context, np.zeros(7)) # If the diagram is only the hardware interface, then we must advance it a # little bit so that first LCM messages get processed. A simulated plant is # already publishing correct positions even without advancing, and indeed # we must not advance a simulated plant until the sliders and filters have # been initialized to match the plant. if args.hardware: simulator.AdvanceTo(1e-6) q0 = station.GetOutputPort("iiwa_position_measured").Eval(station_context) differential_ik.parameters.set_nominal_joint_position(q0) teleop.SetPose(differential_ik.ForwardKinematics(q0)) filter_.set_initial_output_value( diagram.GetMutableSubsystemContext( filter_, simulator.get_mutable_context()), teleop.get_output_port(0).Eval(diagram.GetMutableSubsystemContext( teleop, simulator.get_mutable_context()))) differential_ik.SetPositions(diagram.GetMutableSubsystemContext( differential_ik, simulator.get_mutable_context()), q0) simulator.set_target_realtime_rate(args.target_realtime_rate) print_instructions() simulator.AdvanceTo(args.duration)
def main(): parser = argparse.ArgumentParser(description=__doc__) parser.add_argument( "--target_realtime_rate", type=float, default=1.0, help="Desired rate relative to real time. See documentation for " "Simulator::set_target_realtime_rate() for details.") parser.add_argument("--duration", type=float, default=np.inf, help="Desired duration of the simulation in seconds.") parser.add_argument( "--hardware", action='store_true', help="Use the ManipulationStationHardwareInterface instead of an " "in-process simulation.") parser.add_argument("--test", action='store_true', help="Disable opening the gui window for testing.") parser.add_argument( "--filter_time_const", type=float, default=0.005, help="Time constant for the first order low pass filter applied to" "the teleop commands") parser.add_argument( "--velocity_limit_factor", type=float, default=1.0, help="This value, typically between 0 and 1, further limits the " "iiwa14 joint velocities. It multiplies each of the seven " "pre-defined joint velocity limits. " "Note: The pre-defined velocity limits are specified by " "iiwa14_velocity_limits, found in this python file.") parser.add_argument('--setup', type=str, default='manipulation_class', help="The manipulation station setup to simulate. ", choices=['manipulation_class', 'clutter_clearing']) MeshcatVisualizer.add_argparse_argument(parser) args = parser.parse_args() if args.test: # Don't grab mouse focus during testing. grab_focus = False # See: https://stackoverflow.com/a/52528832/7829525 os.environ["SDL_VIDEODRIVER"] = "dummy" else: grab_focus = True builder = DiagramBuilder() if args.hardware: station = builder.AddSystem(ManipulationStationHardwareInterface()) station.Connect(wait_for_cameras=False) else: station = builder.AddSystem(ManipulationStation()) # Initializes the chosen station type. if args.setup == 'manipulation_class': station.SetupManipulationClassStation() station.AddManipulandFromFile( ("drake/examples/manipulation_station/models/" "061_foam_brick.sdf"), RigidTransform(RotationMatrix.Identity(), [0.6, 0, 0])) elif args.setup == 'clutter_clearing': station.SetupClutterClearingStation() ycb_objects = CreateClutterClearingYcbObjectList() for model_file, X_WObject in ycb_objects: station.AddManipulandFromFile(model_file, X_WObject) station.Finalize() ConnectDrakeVisualizer(builder, station.get_scene_graph(), station.GetOutputPort("pose_bundle")) if args.meshcat: meshcat = builder.AddSystem( MeshcatVisualizer(station.get_scene_graph(), zmq_url=args.meshcat)) builder.Connect(station.GetOutputPort("pose_bundle"), meshcat.get_input_port(0)) robot = station.get_controller_plant() params = DifferentialInverseKinematicsParameters(robot.num_positions(), robot.num_velocities()) time_step = 0.005 params.set_timestep(time_step) # True velocity limits for the IIWA14 (in rad, rounded down to the first # decimal) iiwa14_velocity_limits = np.array([1.4, 1.4, 1.7, 1.3, 2.2, 2.3, 2.3]) # Stay within a small fraction of those limits for this teleop demo. factor = args.velocity_limit_factor params.set_joint_velocity_limits( (-factor * iiwa14_velocity_limits, factor * iiwa14_velocity_limits)) differential_ik = builder.AddSystem( DifferentialIK(robot, robot.GetFrameByName("iiwa_link_7"), params, time_step)) builder.Connect(differential_ik.GetOutputPort("joint_position_desired"), station.GetInputPort("iiwa_position")) teleop = builder.AddSystem(MouseKeyboardTeleop(grab_focus=grab_focus)) filter_ = builder.AddSystem( FirstOrderLowPassFilter(time_constant=args.filter_time_const, size=6)) builder.Connect(teleop.get_output_port(0), filter_.get_input_port(0)) builder.Connect(filter_.get_output_port(0), differential_ik.GetInputPort("rpy_xyz_desired")) builder.Connect(teleop.GetOutputPort("position"), station.GetInputPort("wsg_position")) builder.Connect(teleop.GetOutputPort("force_limit"), station.GetInputPort("wsg_force_limit")) diagram = builder.Build() simulator = Simulator(diagram) # This is important to avoid duplicate publishes to the hardware interface: simulator.set_publish_every_time_step(False) station_context = diagram.GetMutableSubsystemContext( station, simulator.get_mutable_context()) station.GetInputPort("iiwa_feedforward_torque").FixValue( station_context, np.zeros(7)) simulator.AdvanceTo(1e-6) q0 = station.GetOutputPort("iiwa_position_measured").Eval(station_context) differential_ik.parameters.set_nominal_joint_position(q0) teleop.SetPose(differential_ik.ForwardKinematics(q0)) filter_.set_initial_output_value( diagram.GetMutableSubsystemContext(filter_, simulator.get_mutable_context()), teleop.get_output_port(0).Eval( diagram.GetMutableSubsystemContext( teleop, simulator.get_mutable_context()))) differential_ik.SetPositions( diagram.GetMutableSubsystemContext(differential_ik, simulator.get_mutable_context()), q0) simulator.set_target_realtime_rate(args.target_realtime_rate) print_instructions() simulator.AdvanceTo(args.duration)
if args.hardware: # TODO(russt): Replace this hard-coded camera serial number with a config # file. camera_ids = ["805212060544"] station = builder.AddSystem( ManipulationStationHardwareInterface(camera_ids)) station.Connect(wait_for_cameras=False) else: station = builder.AddSystem(ManipulationStation()) # Initializes the chosen station type. if args.setup == 'manipulation_class': station.SetupManipulationClassStation() station.AddManipulandFromFile( "drake/examples/manipulation_station/models/061_foam_brick.sdf", RigidTransform(RotationMatrix.Identity(), [0.6, 0, 0])) elif args.setup == 'clutter_clearing': station.SetupClutterClearingStation() ycb_objects = CreateClutterClearingYcbObjectList() for model_file, X_WObject in ycb_objects: station.AddManipulandFromFile(model_file, X_WObject) elif args.setup == 'planar': station.SetupPlanarIiwaStation() station.AddManipulandFromFile( "drake/examples/manipulation_station/models/061_foam_brick.sdf", RigidTransform(RotationMatrix.Identity(), [0.6, 0, 0])) station.Finalize() ConnectDrakeVisualizer(builder, station.get_scene_graph(), station.GetOutputPort("pose_bundle"))
def InverseKinPointwise( p_WQ_start, p_WQ_end, duration, num_knot_points, q_initial_guess, InterpolatePosition=None, InterpolateOrientation=None, position_tolerance=0.005, theta_bound=0.005 * np.pi, # 0.9 degrees is_printing=True): """ Calculates a joint space trajectory for iiwa by repeatedly calling IK. The first IK is initialized (seeded) with q_initial_guess. Subsequent IKs are seeded with the solution from the previous IK. Positions for point Q (p_EQ) and orientations for the end effector, generated respectively by InterpolatePosition and InterpolateOrientation, are added to the IKs as constraints. @param p_WQ_start: The first argument of function InterpolatePosition (defined below). @param p_WQ_end: The second argument of function InterpolatePosition (defined below). @param duration: The duration of the trajectory returned by this function in seconds. @param num_knot_points: number of knot points in the trajectory. @param q_initial_guess: initial guess for the first IK. @param InterpolatePosition: A function with signature (start, end, num_knot_points, i). It returns p_WQ, a (3,) numpy array which describes the desired position of Q at knot point i in world frame. @param InterpolateOrientation: A function with signature @param position_tolerance: tolerance for IK position constraints in meters. @param theta_bound: tolerance for IK orientation constraints in radians. @param is_printing: whether the solution results of IKs are printed. @return: qtraj: a 7-dimensional cubic polynomial that describes a trajectory for the iiwa arm. @return: q_knots: a (n, num_knot_points) numpy array (where n = plant.num_positions()) that stores solutions returned by all IKs. It can be used to initialize IKs for the next trajectory. """ q_knots = np.zeros((num_knot_points + 1, plant.num_positions())) q_knots[0] = q_initial_guess for i in range(num_knot_points): ik = inverse_kinematics.InverseKinematics(plant) q_variables = ik.q() # Orientation constraint R_WL7_ref = InterpolateOrientation(i, num_knot_points) ik.AddOrientationConstraint(frameAbar=world_frame, R_AbarA=R_WL7_ref, frameBbar=l7_frame, R_BbarB=RotationMatrix.Identity(), theta_bound=theta_bound) # Position constraint p_WQ = InterpolatePosition(p_WQ_start, p_WQ_end, num_knot_points, i) ik.AddPositionConstraint(frameB=l7_frame, p_BQ=p_L7Q, frameA=world_frame, p_AQ_lower=p_WQ - position_tolerance, p_AQ_upper=p_WQ + position_tolerance) prog = ik.prog() # use the robot posture at the previous knot point as # an initial guess. prog.SetInitialGuess(q_variables, q_knots[i]) result = prog.Solve() if is_printing: print i, ": ", result q_knots[i + 1] = prog.GetSolution(q_variables) t_knots = np.linspace(0, duration, num_knot_points + 1) q_knots_kuka = GetKukaQKnots(q_knots) qtraj = PiecewisePolynomial.Cubic(t_knots, q_knots_kuka.T, np.zeros(7), np.zeros(7)) return qtraj, q_knots
def InverseKinPointwise(InterpolatePosition, InterpolateOrientation, p_L7Q, duration, num_knot_points, q_initial_guess, position_tolerance=0.005, theta_bound=0.005 * np.pi): """ Calculates a joint space trajectory for iiwa by repeatedly calling IK. The returned trajectory has (num_knot_points) knot points. To improve the continuity of the trajectory, the IK from which q_knots[i] is solved is initialized with q_knots[i-1]. Positions for point Q (p_EQ) and orientations for the end effector, generated respectively by InterpolatePosition and InterpolateOrientation, are added to the IKs as constraints. :param InterpolatePosition: A function with signature (tau) with 0 <= tau <= 1. It returns p_WQ, a (3,) numpy array which describes the desired position of Q. For example, to get p_WQ for knot point i, use tau = i / (num_knot_points - 1). :param InterpolateOrientation: A function with the same signature as InterpolatePosition. It returns R_WL7, a RotationMatrix which describes the desired orientation of frame L7. :param num_knot_points: number of knot points in the trajectory. :param q_initial_guess: initial guess for the first IK. :param position_tolerance: tolerance for IK position constraints in meters. :param theta_bound: tolerance for IK orientation constraints in radians. :param is_printing: whether the solution results of IKs are printed. :return: qtraj: a 7-dimensional cubic polynomial that describes a trajectory for the iiwa arm. :return: q_knots: a (n, num_knot_points) numpy array (where n = plant.num_positions()) that stores solutions returned by all IKs. It can be used to initialize IKs for the next trajectory. """ q_knots = np.zeros((num_knot_points, plant.num_positions())) ################### Your code here ################### for i in range(num_knot_points): ik = inverse_kinematics.InverseKinematics(plant) q_variables = ik.q() # Orientation constraint R_WL7_ref = InterpolateOrientation(i / (num_knot_points - 1)) ik.AddOrientationConstraint( frameAbar=world_frame, R_AbarA=R_WL7_ref, frameBbar=l7_frame, R_BbarB=RotationMatrix.Identity(), theta_bound=theta_bound) # Position constraint p_WQ = InterpolatePosition(i / (num_knot_points - 1)) ik.AddPositionConstraint( frameB=l7_frame, p_BQ=p_L7Q, frameA=world_frame, p_AQ_lower=p_WQ - position_tolerance, p_AQ_upper=p_WQ + position_tolerance) prog = ik.prog() # use the robot configuration at the previous knot point as # an initial guess. if i > 0: prog.SetInitialGuess(q_variables, q_knots[i-1]) else: prog.SetInitialGuess(q_variables, q_initial_guess) result = mp.Solve(prog) # throw if no solution found. if result.get_solution_result() != SolutionResult.kSolutionFound: print(i, result.get_solution_result()) raise RuntimeError q_knots[i] = result.GetSolution(q_variables) t_knots = np.linspace(0, duration, num_knot_points) qtraj = PiecewisePolynomial.Cubic( t_knots, q_knots.T, np.zeros(7), np.zeros(7)) return qtraj, q_knots ######################################################## raise NotImplementedError