def setUp(self): self.q0 = [0, 0, 0, -1.75, 0, 1.0, 0] self.time_step = 2e-3 self.prevdir = os.getcwd() os.chdir(os.path.expanduser("pddl_planning")) task, diagram, state_machine = load_dope(time_step=self.time_step, dope_path="poses.txt", goal_name="soup", is_visualizing=False) plant = task.mbp task.publish() context = diagram.GetMutableSubsystemContext(plant, task.diagram_context) world_frame = plant.world_frame() tree = plant.tree() X_WSoup = tree.CalcRelativeTransform( context, frame_A=world_frame, frame_B=plant.GetFrameByName("base_link_soup")) self.manip_station_sim = ManipulationStationSimulator( time_step=self.time_step, object_file_path="./models/ycb_objects/soup_can.sdf", object_base_link_name="base_link_soup", X_WObject=X_WSoup)
def test_open_door_by_impedance_and_position(self): modes = ("Impedance", "Position") is_plan_runner_diagram_list = [False, True] for is_plan_runner_diagram in is_plan_runner_diagram_list: for mode in modes: # Create simulator manip_station_sim = ManipulationStationSimulator( time_step=2e-3) plan_list, gripper_setpoint_list = \ GenerateOpenLeftDoorPlansByImpedanceOrPosition( open_door_method=mode, is_open_fully=True) # Make a copy of the initial position of the plans passed to PlanRunner. q_iiwa_beginning = plan_list[0].traj.value(0).flatten() iiwa_position_command_log, iiwa_position_measured_log, iiwa_external_torque_log, \ plant_state_log, t_plan = manip_station_sim.RunSimulation( plan_list, gripper_setpoint_list, extra_time=2.0, real_time_rate=0.0, q0_kuka=self.q0, is_visualizing=False, is_plan_runner_diagram=is_plan_runner_diagram) # Run tests self.InspectLog(plant_state_log, manip_station_sim.plant) self.assertTrue( self.HasReturnedToQtarget(q_iiwa_beginning, plant_state_log, manip_station_sim.plant))
def test_open_door_by_trajectory(self): # Create simulator and run simulation manip_station_sim = ManipulationStationSimulator(time_step=2e-3) plan_list, gripper_setpoint_list = \ GenerateOpenLeftDoorPlansByTrajectory() iiwa_position_command_log, iiwa_position_measured_log, iiwa_external_torque_log, \ plant_state_log = manip_station_sim.RunSimulation( plan_list, gripper_setpoint_list, extra_time=2.0, real_time_rate=0.0, q0_kuka=self.q0, is_visualizing=False) # Run tests self.InspectLog(plant_state_log, manip_station_sim.plant)
def test_open_door_by_trajectory(self): is_plan_runner_diagram_list = [True, False] for is_plan_runner_diagram in is_plan_runner_diagram_list: plan_list, gripper_setpoint_list = \ GenerateOpenLeftDoorPlansByTrajectory() # Create simulator manip_station_sim = ManipulationStationSimulator(time_step=2e-3) iiwa_position_command_log, iiwa_position_measured_log, iiwa_external_torque_log, \ plant_state_log, t_plan = manip_station_sim.RunSimulation( plan_list, gripper_setpoint_list, extra_time=2.0, real_time_rate=0.0, q0_kuka=self.q0, is_visualizing=False, is_plan_runner_diagram=is_plan_runner_diagram) # Run tests self.InspectLog(plant_state_log, manip_station_sim.plant)
print "Estimated right door angle: " + str(estimated_right_door_angle) left_door_angle = -estimated_left_door_angle rotation_matrix = np.array([[np.cos(left_door_angle), -np.sin(left_door_angle), 0], [np.sin(left_door_angle), np.cos(left_door_angle), 0], [0, 0, 1]]) p_handle_2_hinge_new = np.dot(rotation_matrix, p_handle_2_hinge) p_LC_handle_new = p_handle_2_hinge_new + p_LC_left_hinge p_WC_handle_new = p_WL + p_LC_handle_new theta0_hinge_new = np.arctan2(np.abs(p_handle_2_hinge_new[0]), np.abs(p_handle_2_hinge_new[1])) # = theta0_hinge - left_door_angle # Construct simulator system. object_file_path = FindResourceOrThrow( "drake/examples/manipulation_station/models/061_foam_brick.sdf") manip_station_sim = ManipulationStationSimulator( time_step=2e-3, object_file_path=object_file_path, object_base_link_name="base_link",) # Generate plans. plan_list = None gripper_setpoint_list = None if args.controller == "Trajectory": plan_list, gripper_setpoint_list = GenerateOpenLeftDoorPlansByTrajectory() elif args.controller == "Impedance" or args.controller == "Position": plan_list, gripper_setpoint_list = GenerateOpenLeftDoorPlansByImpedanceOrPosition( open_door_method=args.controller, is_open_fully=args.open_fully, handle_angle_start=theta0_hinge_new, handle_position=p_WC_handle_new, is_hardware=args.hardware) print type(gripper_setpoint_list), gripper_setpoint_list # Run simulator (simulation or hardware). if is_hardware:
default=False, help="Turns off visualization") parser.add_argument( "--profiling", action="store_true", default=False, help="profile RunSimulation or RunRealRobot with cProfile") parser.add_argument("--diagram_plan_runner", action="store_true", default=False, help="Use the diagram version of plan_runner") args = parser.parse_args() is_hardware = args.hardware # Construct simulator system. manip_station_sim = ManipulationStationSimulator(time_step=2e-3) # Generate plans. plan_list, gripper_setpoint_list = GenerateExampleJointAndTaskSpacePlans() # Run simulator (simulation or hardware). if is_hardware: if args.profiling: cProfile.run( "manip_station_sim.RunRealRobot(\ plan_list, gripper_setpoint_list,\ is_plan_runner_diagram=args.diagram_plan_runner)", "stats_hardware") else: iiwa_position_command_log, iiwa_position_measured_log, iiwa_velocity_estimated_log, \ iiwa_external_torque_log, t_plan = \
def build(self, real_time_rate=0, is_visualizing=False): # Create manipulation station simulator self.manip_station_sim = ManipulationStationSimulator( time_step=5e-3, object_file_path=object_file_path, object_base_link_name="base_link", ) # Create plan runner plan_runner, self.plan_scheduler = CreateManipStationPlanRunnerDiagram( station=self.manip_station_sim.station, kuka_plans=[], gripper_setpoint_list=[], rl_environment=True) self.manip_station_sim.plan_runner = plan_runner # Create builder and add systems builder = DiagramBuilder() builder.AddSystem(self.manip_station_sim.station) builder.AddSystem(plan_runner) # Connect systems builder.Connect( plan_runner.GetOutputPort("gripper_setpoint"), self.manip_station_sim.station.GetInputPort("wsg_position")) builder.Connect( plan_runner.GetOutputPort("force_limit"), self.manip_station_sim.station.GetInputPort("wsg_force_limit")) demux = builder.AddSystem(Demultiplexer(14, 7)) builder.Connect( plan_runner.GetOutputPort("iiwa_position_and_torque_command"), demux.get_input_port(0)) builder.Connect( demux.get_output_port(0), self.manip_station_sim.station.GetInputPort("iiwa_position")) builder.Connect( demux.get_output_port(1), self.manip_station_sim.station.GetInputPort( "iiwa_feedforward_torque")) builder.Connect( self.manip_station_sim.station.GetOutputPort( "iiwa_position_measured"), plan_runner.GetInputPort("iiwa_position")) builder.Connect( self.manip_station_sim.station.GetOutputPort( "iiwa_velocity_estimated"), plan_runner.GetInputPort("iiwa_velocity")) # Add meshcat visualizer if is_visualizing: scene_graph = self.manip_station_sim.station.get_mutable_scene_graph( ) viz = MeshcatVisualizer(scene_graph, is_drawing_contact_force=False, plant=self.manip_station_sim.plant) builder.AddSystem(viz) builder.Connect( self.manip_station_sim.station.GetOutputPort("pose_bundle"), viz.GetInputPort("lcm_visualization")) # Build diagram self.diagram = builder.Build() if is_visualizing: print("Setting up visualizer...") viz.load() time.sleep(2.0) # Construct Simulator self.simulator = Simulator(self.diagram) self.manip_station_sim.simulator = self.simulator self.simulator.set_publish_every_time_step(False) self.simulator.set_target_realtime_rate(real_time_rate) self.context = self.diagram.GetMutableSubsystemContext( self.manip_station_sim.station, self.simulator.get_mutable_context()) self.left_hinge_joint = self.manip_station_sim.plant.GetJointByName( "left_door_hinge") self.right_hinge_joint = self.manip_station_sim.plant.GetJointByName( "right_door_hinge") # Goal for training self.goal_position = np.array([0.85, 0, 0.31]) # Object body self.obj = self.manip_station_sim.plant.GetBodyByName( self.manip_station_sim.object_base_link_name, self.manip_station_sim.object) # Properties for RL max_action = np.ones(8) * 0.1 max_action[-1] = 0.03 low_action = -1 * max_action low_action[-1] = 0 self.action_space = ActionSpace(low=low_action, high=max_action) self.state_dim = self._getObservation().shape[0] self._episode_steps = 0 self._max_episode_steps = 75 # Set initial state of the robot self.reset_sim = False self.reset()
class ManipStationEnvironment(object): def __init__(self, real_time_rate=0, is_visualizing=False): # Store for continuity self.is_visualizing = is_visualizing self.real_time_rate = real_time_rate self.build(real_time_rate=0, is_visualizing=is_visualizing) def build(self, real_time_rate=0, is_visualizing=False): # Create manipulation station simulator self.manip_station_sim = ManipulationStationSimulator( time_step=5e-3, object_file_path=object_file_path, object_base_link_name="base_link", ) # Create plan runner plan_runner, self.plan_scheduler = CreateManipStationPlanRunnerDiagram( station=self.manip_station_sim.station, kuka_plans=[], gripper_setpoint_list=[], rl_environment=True) self.manip_station_sim.plan_runner = plan_runner # Create builder and add systems builder = DiagramBuilder() builder.AddSystem(self.manip_station_sim.station) builder.AddSystem(plan_runner) # Connect systems builder.Connect( plan_runner.GetOutputPort("gripper_setpoint"), self.manip_station_sim.station.GetInputPort("wsg_position")) builder.Connect( plan_runner.GetOutputPort("force_limit"), self.manip_station_sim.station.GetInputPort("wsg_force_limit")) demux = builder.AddSystem(Demultiplexer(14, 7)) builder.Connect( plan_runner.GetOutputPort("iiwa_position_and_torque_command"), demux.get_input_port(0)) builder.Connect( demux.get_output_port(0), self.manip_station_sim.station.GetInputPort("iiwa_position")) builder.Connect( demux.get_output_port(1), self.manip_station_sim.station.GetInputPort( "iiwa_feedforward_torque")) builder.Connect( self.manip_station_sim.station.GetOutputPort( "iiwa_position_measured"), plan_runner.GetInputPort("iiwa_position")) builder.Connect( self.manip_station_sim.station.GetOutputPort( "iiwa_velocity_estimated"), plan_runner.GetInputPort("iiwa_velocity")) # Add meshcat visualizer if is_visualizing: scene_graph = self.manip_station_sim.station.get_mutable_scene_graph( ) viz = MeshcatVisualizer(scene_graph, is_drawing_contact_force=False, plant=self.manip_station_sim.plant) builder.AddSystem(viz) builder.Connect( self.manip_station_sim.station.GetOutputPort("pose_bundle"), viz.GetInputPort("lcm_visualization")) # Build diagram self.diagram = builder.Build() if is_visualizing: print("Setting up visualizer...") viz.load() time.sleep(2.0) # Construct Simulator self.simulator = Simulator(self.diagram) self.manip_station_sim.simulator = self.simulator self.simulator.set_publish_every_time_step(False) self.simulator.set_target_realtime_rate(real_time_rate) self.context = self.diagram.GetMutableSubsystemContext( self.manip_station_sim.station, self.simulator.get_mutable_context()) self.left_hinge_joint = self.manip_station_sim.plant.GetJointByName( "left_door_hinge") self.right_hinge_joint = self.manip_station_sim.plant.GetJointByName( "right_door_hinge") # Goal for training self.goal_position = np.array([0.85, 0, 0.31]) # Object body self.obj = self.manip_station_sim.plant.GetBodyByName( self.manip_station_sim.object_base_link_name, self.manip_station_sim.object) # Properties for RL max_action = np.ones(8) * 0.1 max_action[-1] = 0.03 low_action = -1 * max_action low_action[-1] = 0 self.action_space = ActionSpace(low=low_action, high=max_action) self.state_dim = self._getObservation().shape[0] self._episode_steps = 0 self._max_episode_steps = 75 # Set initial state of the robot self.reset_sim = False self.reset() def step(self, action): assert len(action) == 8 next_plan = JointSpacePlanRelative(delta_q=action[:-1], duration=0.1) sim_duration = self.plan_scheduler.setNextPlan(next_plan, 0.05) try: self.simulator.StepTo(sim_duration) except: self.reset_sim = True return self._getObservation(), -999, True, None reward = self._getReward() if reward > -0.1: done = True else: self._episode_steps += 1 if self._episode_steps == self._max_episode_steps: done = True else: done = False return self._getObservation(), reward, done, None def reset(self): if self.reset_sim: print("Resetting") self.build(real_time_rate=self.real_time_rate, is_visualizing=self.is_visualizing) while True: p_WQ_new = np.random.uniform(low=[0.05, -0.1, 0.5], high=[0.5, 0.1, 0.5]) # p_WQ_new = np.array([0.2, 0, 0.5]) passed, q_home_full = GetConfiguration(p_WQ_new) if passed: break q_home_kuka = GetKukaQKnots(q_home_full)[0] # set initial hinge angles of the cupboard. # setting hinge angle to exactly 0 or 90 degrees will result in intermittent contact # with small contact forces between the door and the cupboard body. self.left_hinge_joint.set_angle( context=self.manip_station_sim.station.GetMutableSubsystemContext( self.manip_station_sim.plant, self.context), angle=-np.pi / 2 + 0.001) self.right_hinge_joint.set_angle( context=self.manip_station_sim.station.GetMutableSubsystemContext( self.manip_station_sim.plant, self.context), angle=np.pi / 2 - 0.001) # set initial pose of the object self.manip_station_sim.SetObjectTranslation(self.goal_position) if self.manip_station_sim.object_base_link_name is not None: self.manip_station_sim.tree.SetFreeBodyPoseOrThrow( self.manip_station_sim.plant.GetBodyByName( self.manip_station_sim.object_base_link_name, self.manip_station_sim.object), self.manip_station_sim.X_WObject, self.manip_station_sim.station.GetMutableSubsystemContext( self.manip_station_sim.plant, self.context)) if self.manip_station_sim.object_base_link_name is not None: self.manip_station_sim.tree.SetFreeBodySpatialVelocityOrThrow( self.manip_station_sim.plant.GetBodyByName( self.manip_station_sim.object_base_link_name, self.manip_station_sim.object), SpatialVelocity(np.zeros(3), np.zeros(3)), self.manip_station_sim.station.GetMutableSubsystemContext( self.manip_station_sim.plant, self.context)) # set initial state of the robot self.manip_station_sim.station.SetIiwaPosition(q_home_kuka, self.context) self.manip_station_sim.station.SetIiwaVelocity(np.zeros(7), self.context) self.manip_station_sim.station.SetWsgPosition(0.02, self.context) self.manip_station_sim.station.SetWsgVelocity(0, self.context) self.simulator.Initialize() self._episode_steps = 0 return self._getObservation() def seed(self, seed): np.random.seed(seed) def _getObservation(self): kuka_position = self.manip_station_sim.station.GetIiwaPosition( self.context) object_position = self.manip_station_sim.tree.EvalBodyPoseInWorld( context=self.manip_station_sim.station.GetMutableSubsystemContext( self.manip_station_sim.plant, self.context), body=self.obj).translation() return np.append(kuka_position, object_position) def _getReward(self): # object_position = self.manip_station_sim.tree.EvalBodyPoseInWorld( # context=self.manip_station_sim.station.GetMutableSubsystemContext(self.manip_station_sim.plant, self.context), # body=self.obj).translation() gripper_pose = self.manip_station_sim.tree.CalcRelativeTransform( context=self.manip_station_sim.station.GetMutableSubsystemContext( self.manip_station_sim.plant, self.context), frame_A=self.manip_station_sim.world_frame, frame_B=self.manip_station_sim.gripper_frame).translation() dist = np.linalg.norm(self.goal_position - gripper_pose) return -dist
def main(): time_step = 2e-3 parser = argparse.ArgumentParser() parser.add_argument('-c', '--cfree', action='store_true', help='Disables collisions when planning') parser.add_argument( '-d', '--deterministic', action='store_true', help='Manually sets the random seeds used by the stream generators') parser.add_argument('-s', '--simulate', action='store_true', help='Simulates the system') parser.add_argument('-e', '--execute', action='store_true', help='Executes the system') parser.add_argument('-l', '--load', action='store_true', help='Loads the last plan') parser.add_argument('-f', '--force_control', action='store_true', help='Use impedance control to open the door') parser.add_argument('-p', '--poses', default=DOPE_PATH, help='The path to the dope poses file') args = parser.parse_args() if args.deterministic: random.seed(0) np.random.seed(0) goal_name = 'soup' if args.poses == 'none': task, diagram, state_machine = load_station(time_step=time_step) else: task, diagram, state_machine = load_dope(time_step=time_step, dope_path=args.poses, goal_name=goal_name) print(task) plant = task.mbp RenderSystemWithGraphviz(diagram) # Useful for getting port names task.publish() context = diagram.GetMutableSubsystemContext(plant, task.diagram_context) world_frame = plant.world_frame() tree = plant.tree() X_WSoup = tree.CalcRelativeTransform( context, frame_A=world_frame, frame_B=plant.GetFrameByName("base_link_soup")) if not args.load: replan(task, context, visualize=True, collisions=not args.cfree, use_impedance=args.force_control) ################################################## if not args.simulate and not args.execute: return manip_station_sim = ManipulationStationSimulator( time_step=time_step, object_file_path=get_sdf_path(goal_name), object_base_link_name="base_link_soup", X_WObject=X_WSoup) plan_list = [] gripper_setpoints = [] splines = np.load("splines.npy") setpoints = np.load("gripper_setpoints.npy") for control, setpoint in zip(splines, setpoints): if isinstance(control, ForceControl): new_plans, new_setpoints = \ GenerateOpenLeftDoorPlansByImpedanceOrPosition("Impedance", is_open_fully=True) plan_list.extend(new_plans) gripper_setpoints.extend(new_setpoints) else: plan_list.append(control.plan()) gripper_setpoints.append(setpoint) dump_plans(plan_list, gripper_setpoints) sim_duration = compute_duration(plan_list) print('Splines: {}\nDuration: {:.3f} seconds'.format( len(plan_list), sim_duration)) if args.execute: raw_input('Execute on hardware?') iiwa_position_command_log, iiwa_position_measured_log, iiwa_external_torque_log = \ manip_station_sim.RunRealRobot(plan_list, gripper_setpoints) #PlotExternalTorqueLog(iiwa_external_torque_log) #PlotIiwaPositionLog(iiwa_position_command_log, iiwa_position_measured_log) else: raw_input('Execute in simulation?') q0 = [0, 0, 0, -1.75, 0, 1.0, 0] iiwa_position_command_log, iiwa_position_measured_log, iiwa_external_torque_log, \ plant_state_log = \ manip_station_sim.RunSimulation(plan_list, gripper_setpoints, extra_time=2.0, real_time_rate=1.0, q0_kuka=q0)
) args = parser.parse_args() object_file_path = FindResourceOrThrow( 'drake/examples/manipulation_station/models/061_foam_brick.sdf' ) obstacle_file_path = "motion_planner/models/cracker_box.sdf" X_WObject = Isometry3.Identity() X_WObject.set_translation([.6, 0, 0]) X_WObstacle = Isometry3.Identity() X_WObstacle.set_translation([0.4, 0, 0]) manip_station_sim = ManipulationStationSimulator( time_step=2e-3, object_file_paths=[object_file_path, obstacle_file_path], object_base_link_names=['base_link', 'base_link_cracker'], X_WObject_list=[X_WObject, X_WObstacle] ) # initial q and goal p q0 = [0, 0, 0, -1.75, 0, 1.0, 0] p_WC_box = np.array([0.6, 0.05 / 2, 0.025 + 0.025]) algo = args.algorithm max_iter = args.max_iter num_runs = args.num_runs motion_planning = MotionPlanning(q_initial=q0, p_goal=p_WC_box, object_file_paths=[object_file_path, obstacle_file_path], object_base_link_names=['base_link', 'base_link_cracker'],
default=300, help="Specify the maximum iterations the algorithm is allowed to run") args = parser.parse_args() object_file_path = FindResourceOrThrow( 'drake/examples/manipulation_station/models/061_foam_brick.sdf') obstacle_file_path = "motion_planner/models/cracker_box.sdf" X_WObject = Isometry3.Identity() X_WObject.set_translation([.6, 0, 0]) X_WObstacle = Isometry3.Identity() X_WObstacle.set_translation([0.4, 0, 0]) manip_station_sim = ManipulationStationSimulator( time_step=2e-3, object_file_paths=[object_file_path, obstacle_file_path], object_base_link_names=['base_link', 'base_link_cracker'], X_WObject_list=[X_WObject, X_WObstacle]) # initial q and goal p q0 = [0, 0, 0, -1.75, 0, 1.0, 0] p_WC_box = np.array([0.6, 0.05 / 2, 0.025 + 0.025]) algo = args.algorithm max_iter = args.max_iter motion_planning = MotionPlanning( q_initial=q0, p_goal=p_WC_box, object_file_paths=[object_file_path, obstacle_file_path], object_base_link_names=['base_link', 'base_link_cracker'],
class TestPDDLPlanning(unittest.TestCase): def setUp(self): self.q0 = [0, 0, 0, -1.75, 0, 1.0, 0] self.time_step = 2e-3 self.prevdir = os.getcwd() os.chdir(os.path.expanduser("pddl_planning")) task, diagram, state_machine = load_dope(time_step=self.time_step, dope_path="poses.txt", goal_name="soup", is_visualizing=False) plant = task.mbp task.publish() context = diagram.GetMutableSubsystemContext(plant, task.diagram_context) world_frame = plant.world_frame() tree = plant.tree() X_WSoup = tree.CalcRelativeTransform( context, frame_A=world_frame, frame_B=plant.GetFrameByName("base_link_soup")) self.manip_station_sim = ManipulationStationSimulator( time_step=self.time_step, object_file_path="./models/ycb_objects/soup_can.sdf", object_base_link_name="base_link_soup", X_WObject=X_WSoup) def tearDown(self): os.chdir(self.prevdir) def InspectLog(self, state_log, plant): tree = plant.tree() data = state_log.data() # create a context of final state. x_final = data[:, -1] context = plant.CreateDefaultContext() x_mutable = tree.GetMutablePositionsAndVelocities(context) x_mutable[:] = x_final # cupboard must be open. hinge_joint = plant.GetJointByName("left_door_hinge") joint_angle = hinge_joint.get_angle(context) self.assertTrue(np.abs(joint_angle) > np.pi/6, "Cupboard door is not fully open.") # velocity must be small throughout the simulation. for x in data.T: v = x[plant.num_positions():] self.assertTrue((np.abs(v) < 3.).all(), "velocity is too large.") def HasReturnedToQtarget(self, q_iiwa_target, state_log, plant): tree = plant.tree() data = state_log.data() q_final = data[:, -1][:plant.num_positions()] iiwa_model = plant.GetModelInstanceByName("iiwa") q_iiwa_final = tree.GetPositionsFromArray(iiwa_model, q_final) return (np.abs(q_iiwa_target - q_iiwa_final) < 0.03).all() def test_pddl(self): splines = np.load("test_data/splines.npy") setpoints = np.load("test_data/gripper_setpoints.npy") plan_list = [] gripper_setpoints = [] for control, setpoint in zip(splines, setpoints): plan_list.append(control.plan()) gripper_setpoints.append(setpoint) sim_duration = compute_duration(plan_list) q_iiwa_beginning = plan_list[0].traj.value(0).flatten() iiwa_position_command_log, iiwa_position_measured_log, iiwa_external_torque_log, \ plant_state_log, t_plan = \ self.manip_station_sim.RunSimulation(plan_list, gripper_setpoints, extra_time=2.0, real_time_rate=0.0, q0_kuka=self.q0, is_visualizing=False) # Run Tests self.InspectLog(plant_state_log, self.manip_station_sim.plant) self.assertTrue( self.HasReturnedToQtarget(q_iiwa_beginning, plant_state_log, self.manip_station_sim.plant)) def test_pddl_force_control(self): splines = np.load("test_data/splines_force_control.npy") setpoints = np.load("test_data/gripper_setpoints_force_control.npy") plan_list = [] gripper_setpoints = [] for control, setpoint in zip(splines, setpoints): if isinstance(control, ForceControl): new_plans, new_setpoints = \ GenerateOpenLeftDoorPlansByImpedanceOrPosition("Impedance", is_open_fully=True) plan_list.extend(new_plans) gripper_setpoints.extend(new_setpoints) else: plan_list.append(control.plan()) gripper_setpoints.append(setpoint) sim_duration = compute_duration(plan_list) q_iiwa_beginning = plan_list[0].traj.value(0).flatten() iiwa_position_command_log, iiwa_position_measured_log, iiwa_external_torque_log, \ plant_state_log, t_plan = \ self.manip_station_sim.RunSimulation(plan_list, gripper_setpoints, extra_time=2.0, real_time_rate=0.0, q0_kuka=self.q0, is_visualizing=False) # Run Tests self.InspectLog(plant_state_log, self.manip_station_sim.plant) self.assertTrue( self.HasReturnedToQtarget(q_iiwa_beginning, plant_state_log, self.manip_station_sim.plant))