def playbackMotion(data1, data2, data3, data4, times): data = np.concatenate((data2, data1, data4, data3), axis=0) tree = RigidBodyTree( FindResource( os.path.dirname(os.path.realpath(__file__)) + "/block_pusher2.urdf"), FloatingBaseType.kFixed) # Set up a block diagram with the robot (dynamics), the controller, and a # visualization block. builder = DiagramBuilder() robot = builder.AddSystem(Player(data, times)) Tview = np.array([[1., 0., 0., 0.], [0., 1., 0., 0.], [0., 0., 0., 1.]], dtype=np.float64) visualizer = builder.AddSystem( PlanarRigidBodyVisualizer(tree, Tview, xlim=[-2.8, 4.8], ylim=[-2.8, 10])) #print(robot.get_output_port(0).size()) builder.Connect(robot.get_output_port(0), visualizer.get_input_port(0)) logger = builder.AddSystem( SignalLogger(tree.get_num_positions() + tree.get_num_velocities())) builder.Connect(robot.get_output_port(0), logger.get_input_port(0)) diagram = builder.Build() # Set up a simulator to run this diagram simulator = Simulator(diagram) simulator.set_target_realtime_rate(1.0) simulator.set_publish_every_time_step(True) # Simulate for 10 seconds simulator.StepTo(times[-1] + 0.5)
def simulateRobot(time, B, v_command): tree = RigidBodyTree( FindResource( os.path.dirname(os.path.realpath(__file__)) + "/block_pusher2.urdf"), FloatingBaseType.kFixed) # Set up a block diagram with the robot (dynamics), the controller, and a # visualization block. builder = DiagramBuilder() robot = builder.AddSystem(RigidBodyPlant(tree)) controller = builder.AddSystem(DController(tree, B, v_command)) builder.Connect(robot.get_output_port(0), controller.get_input_port(0)) builder.Connect(controller.get_output_port(0), robot.get_input_port(0)) Tview = np.array([[1., 0., 0., 0.], [0., 1., 0., 0.], [0., 0., 0., 1.]], dtype=np.float64) visualizer = builder.AddSystem( PlanarRigidBodyVisualizer(tree, Tview, xlim=[-2.8, 4.8], ylim=[-2.8, 10])) builder.Connect(robot.get_output_port(0), visualizer.get_input_port(0)) logger = builder.AddSystem( SignalLogger(tree.get_num_positions() + tree.get_num_velocities())) builder.Connect(robot.get_output_port(0), logger.get_input_port(0)) diagram = builder.Build() # Set up a simulator to run this diagram simulator = Simulator(diagram) simulator.set_target_realtime_rate(1.0) simulator.set_publish_every_time_step(True) # Set the initial conditions context = simulator.get_mutable_context() state = context.get_mutable_continuous_state_vector() start1 = 3 * np.pi / 16 start2 = 15 * np.pi / 16 #np.pi/6 - eps, 2*np.pi/3 + eps, -np.pi/6 + eps, -2*np.pi/3 - eps, np.pi/6 - eps, 2*np.pi/3 + eps, -np.pi/6 + eps, -2*np.pi/3 - eps state.SetFromVector( (start1, start2, -start1, -start2, np.pi + start1, start2, np.pi - start1, -start2, 1, 1, 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0.)) # (theta1, theta2, theta1dot, theta2dot) # Simulate for 10 seconds simulator.StepTo(time) #import pdb; pdb.set_trace() return (logger.data()[8:11, :], logger.data()[:8, :], logger.data()[19:22, :], logger.data()[11:19, :], logger.sample_times())
def do_cut(self, rbt, x, cut_body_index, cut_pt, cut_normal): # Rebuilds the full rigid body tree, replacing cut_body_index # with one that is cut, but otherwise keeping the rest of the # tree the same. The new tree won't have the same body indices # (for the manipulands and anything added after them) as the # original. old_manipuland_indices = deepcopy(self.manipuland_body_indices) old_manipuland_params = deepcopy(self.manipuland_params) self.__init__() new_rbt = RigidBodyTree() self.setup_kuka(new_rbt) q_new = np.zeros(rbt.get_num_positions() + 6) v_new = np.zeros(rbt.get_num_positions() + 6) if rbt.get_num_positions() != rbt.get_num_velocities(): raise Exception("Can't handle nq != nv, sorry...") def copy_state(from_indices, to_indices): q_new[to_indices] = x[from_indices] v_new[to_indices] = x[np.array(from_indices) + rbt.get_num_positions()] copy_state(range(new_rbt.get_num_positions()), range(new_rbt.get_num_positions())) k = 0 for i, ind in enumerate(old_manipuland_indices): print i, ind p = old_manipuland_params[i] if ind is cut_body_index: for sign in [-1., 1.]: try: self.add_cut_cylinder_to_tabletop( new_rbt, height=p["height"], radius=p["radius"], cut_dirs=p["cut_dirs"] + [cut_normal * sign], cut_points=p["cut_points"] + [cut_pt + cut_normal * sign * 0.002]) except subprocess.CalledProcessError as e: print "Failed a cut: ", e continue # failed to cut k += 1 copy_state( range( rbt.get_body(ind).get_position_start_index(), rbt.get_body(ind).get_position_start_index() + 6), range(new_rbt.get_num_positions() - 6, new_rbt.get_num_positions())) else: self.add_cut_cylinder_to_tabletop(new_rbt, height=p["height"], radius=p["radius"], cut_dirs=p["cut_dirs"], cut_points=p["cut_points"]) copy_state( range( rbt.get_body(ind).get_position_start_index(), rbt.get_body(ind).get_position_start_index() + 6), range(new_rbt.get_num_positions() - 6, new_rbt.get_num_positions())) k += 1 # Account for possible cut failures q_new = q_new[:new_rbt.get_num_positions()] v_new = v_new[:new_rbt.get_num_velocities()] # Map old state into new state new_rbt.compile() q_new = self.project_rbt_to_nearest_feasible_on_table(new_rbt, q_new) return new_rbt, np.hstack([q_new, v_new])
def getPredictedMotion(B, v_command, time): #object_positions = object_positions + 0.1 #manipulator_positions = manipulator_positions + 0.1 #object_velocities = object_velocities + 0.1 #manipulator_velocities = manipulator_velocities + 0.1 #object_positions = object_positions[:, range(0,object_positions.shape[1],2)] #manipulator_positions = manipulator_positions[:, range(0,manipulator_positions.shape[1],2)] #object_velocities = object_velocities[:, range(0,object_velocities.shape[1],2)] #manipulator_velocities = manipulator_velocities[:, range(0,manipulator_velocities.shape[1],2)] #times = times[range(0,times.size,2)] #import pdb; pdb.set_trace() step = 0.01 A = 10 * np.eye(3) tree = RigidBodyTree( FindResource( os.path.dirname(os.path.realpath(__file__)) + "/block_pusher2.urdf"), FloatingBaseType.kFixed) # Set up a block diagram with the robot (dynamics), the controller, and a # visualization block. builder = DiagramBuilder() #robot = builder.AddSystem(RigidBodyPlant(tree)) robot = builder.AddSystem( QuasiStaticRigidBodyPlant(tree, step, A, np.linalg.inv(B))) controller = builder.AddSystem(QController(tree, v_command, step)) #builder.Connect(robot.get_output_port(0), controller.get_input_port(0)) builder.Connect(controller.get_output_port(0), robot.get_input_port(0)) Tview = np.array([[1., 0., 0., 0.], [0., 1., 0., 0.], [0., 0., 0., 1.]], dtype=np.float64) visualizer = builder.AddSystem( PlanarRigidBodyVisualizer(tree, Tview, xlim=[-2.8, 4.8], ylim=[-2.8, 10])) builder.Connect(robot.get_output_port(0), visualizer.get_input_port(0)) logger = builder.AddSystem( SignalLogger(tree.get_num_positions() + tree.get_num_velocities())) builder.Connect(robot.get_output_port(0), logger.get_input_port(0)) diagram = builder.Build() # Set up a simulator to run this diagram simulator = Simulator(diagram) simulator.set_target_realtime_rate(1.0) simulator.set_publish_every_time_step(True) # Set the initial conditions context = simulator.get_mutable_context() state = context.get_mutable_discrete_state_vector() start1 = 3 * np.pi / 16 start2 = 15 * np.pi / 16 #np.pi/6 - eps, 2*np.pi/3 + eps, -np.pi/6 + eps, -2*np.pi/3 - eps, np.pi/6 - eps, 2*np.pi/3 + eps, -np.pi/6 + eps, -2*np.pi/3 - eps state.SetFromVector( (start1, start2, -start1, -start2, np.pi + start1, start2, np.pi - start1, -start2, 1, 1, 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0.)) # (theta1, theta2, theta1dot, theta2dot) # Simulate for 10 seconds simulator.StepTo(time) #import pdb; pdb.set_trace() return (logger.data()[8:11, :], logger.data()[:8, :], logger.data()[19:22, :], logger.data()[11:19, :], logger.sample_times())
builder = DiagramBuilder() # build the experiment system tree = RigidBodyTree() AddFlatTerrainToWorld(tree, 100, 10) addKukaSetup(tree,collision_type='spheres') # create the diagram plant = RigidBodyPlant(tree, 0.0001) # 2000 Hz deltas = [-0.66667,-0.33333,0.0,0.5,1.0] for delta in deltas: # print "delta: " + str(delta) for i in range(tree.get_num_positions()+tree.get_num_velocities()): # print "index: " + str(i) xk = np.zeros(tree.get_num_positions()+tree.get_num_velocities()) # xk = np.array([0.3689,0.4607,0.9816,0.1564,0.8555,0.6448,0.3763,0.1909,0.4283,0.4820,0.1206,0.5895,0.2262,0.3846]) xk[i] += delta str_out = '' for j in range(tree.get_num_positions()+tree.get_num_velocities()): str_out += "%.6f " % xk[j] print str_out + "\n" kinsol = tree.doKinematics(xk[:tree.get_num_positions()],xk[tree.get_num_positions():]) massMatrix = tree.massMatrix(kinsol) biasTerm = tree.dynamicsBiasTerm(kinsol,{}) str_out = '' for j in range(tree.get_num_positions()): for k in range(tree.get_num_positions()): str_out += "%.4f " % massMatrix[j][k]
# tree = RigidBodyTree(FindResource("cubli/cubli.sdf"), # FloatingBaseType.kFixed) builder = DiagramBuilder() # AddModelInstancesFromSdfString( # open("underactuated/src/cubli/cubli.sdf", 'r').read(), # FloatingBaseType.kFixed, # None, tree) # tree tree = RigidBodyTree(FindResource("cubli/cubli.urdf"), FloatingBaseType.kFixed) # # AddFlatTerrainToWorld(tree, 1000, -1) # plant = RigidBodyPlant(tree, timestep) plant = RigidBodyPlant(tree) nx = tree.get_num_positions() + tree.get_num_velocities() allmaterials = CompliantMaterial() allmaterials.set_youngs_modulus(1E9) # default 1E9 allmaterials.set_dissipation(1.0) # default 0.32 allmaterials.set_friction(1.0) # default 0.9. plant.set_default_compliant_material(allmaterials) context = plant.CreateDefaultContext() print(tree.get_num_positions()) # ETHAN robot = builder.AddSystem(plant) # builder.ExportInput(robot.get_input_port(0))