def build_pancake_flipper_plant(builder): '''Creates a pancake_flipper MultibodyPlant. Returns the plant and corresponding scene graph. Inputs: builder -- a DiagramBuilder Outputs: the MultibodyPlant object and corresponding SceneGraph representing the pancake flipper ''' # Instantiate the pancake flipper plant and the scene graph. # The scene graph is a container for the geometries of all # the physical systems in our diagram pancake_flipper, scene_graph = AddMultibodyPlantSceneGraph( builder, time_step=0.0 # discrete update period, or zero for continuous systems ) # parse the urdf and populate the plant urdf_path = './models/pancake_flipper_massless_links_nofriction.urdf' Parser(pancake_flipper).AddModelFromFile(urdf_path) # Finalize the plant so we can use it pancake_flipper.Finalize() # Return the plant and scene graph, as promised return pancake_flipper, scene_graph
def runPendulumExample(args): builder = DiagramBuilder() plant, scene_graph = AddMultibodyPlantSceneGraph(builder) parser = Parser(plant) parser.AddModelFromFile(FindResource("pendulum/pendulum.urdf")) plant.Finalize() pose_bundle_output_port = scene_graph.get_pose_bundle_output_port() Tview = np.array([[1., 0., 0., 0.], [0., 0., 1., 0.], [0., 0., 0., 1.]], dtype=np.float64) visualizer = builder.AddSystem( PlanarSceneGraphVisualizer(scene_graph, Tview=Tview, xlim=[-1.2, 1.2], ylim=[-1.2, 1.2])) builder.Connect(pose_bundle_output_port, visualizer.get_input_port(0)) diagram = builder.Build() simulator = Simulator(diagram) simulator.Initialize() simulator.set_target_realtime_rate(1.0) # Fix the input port to zero. plant_context = diagram.GetMutableSubsystemContext( plant, simulator.get_mutable_context()) plant_context.FixInputPort(plant.get_actuation_input_port().get_index(), np.zeros(plant.num_actuators())) plant_context.SetContinuousState([0.5, 0.1]) simulator.StepTo(args.duration)
def main(): builder = DiagramBuilder() plant, scene_graph = AddMultibodyPlantSceneGraph(builder, 1e-3) body = add_body(plant, "welded_body") add_geometry(plant, body) plant.WeldFrames(plant.world_frame(), body.body_frame()) body = add_body(plant, "floating_body") add_geometry(plant, body) plant.SetDefaultFreeBodyPose(body, RigidTransform([1., 0, 1.])) plant.Finalize() ConnectDrakeVisualizer(builder, scene_graph) diagram = builder.Build() simulator = Simulator(diagram) context = simulator.get_context() # plant.SetFreeBodyPose(context, body, X_WB) # plant.SetFreeBodySpatialVelocity(body, V_WB, context) # Should look at, show 40sec to 50sec. # TODO(eric.cousineau): Without reset stats, it freezes? :( # simulator.AdvanceTo(40.) simulator.set_target_realtime_rate(100.) # simulator.ResetStatistics() dt = 0.1 while context.get_time() < 240.: simulator.AdvanceTo(context.get_time() + dt)
def RunSimulation(self, real_time_rate=1.0): ''' Here we test using the NNSystem in a Simulator to drive an acrobot. ''' sdf_file = "assets/acrobot.sdf" urdf_file = "assets/acrobot.urdf" builder = DiagramBuilder() plant, scene_graph = AddMultibodyPlantSceneGraph(builder) parser = Parser(plant=plant, scene_graph=scene_graph) parser.AddModelFromFile(sdf_file) plant.Finalize(scene_graph) # Add nn_system = NNSystem(self.pytorch_nn_object) builder.AddSystem(nn_system) # NN -> plant builder.Connect(nn_system.NN_out_output_port, plant.get_actuation_input_port()) # plant -> NN builder.Connect(plant.get_continuous_state_output_port(), nn_system.NN_in_input_port) # Add meshcat visualizer meshcat = MeshcatVisualizer(scene_graph) builder.AddSystem(meshcat) # builder.Connect(scene_graph.GetOutputPort("lcm_visualization"), builder.Connect(scene_graph.get_pose_bundle_output_port(), meshcat.GetInputPort("lcm_visualization")) # build diagram diagram = builder.Build() meshcat.load() # time.sleep(2.0) RenderSystemWithGraphviz(diagram) # construct simulator simulator = Simulator(diagram) # context = diagram.GetMutableSubsystemContext( # self.station, simulator.get_mutable_context()) simulator.set_publish_every_time_step(False) simulator.set_target_realtime_rate(real_time_rate) simulator.Initialize() sim_duration = 5. simulator.StepTo(sim_duration) print("stepping complete")
def setup_dot_diagram(builder, args): ''' Using an existing DiagramBuilder, adds a sim of the Dot robot. Args comes from argparse. The returned controller will need its first port connected to a setpoint source.''' print( "TODO: load in servo calibration dict to a servo calibration object that gets shared" ) with open(args.yaml_path, "r") as f: config_dict = yaml.load(f, Loader=Loader) sdf_path = os.path.join(os.path.dirname(args.yaml_path), config_dict["sdf_path"]) plant, scene_graph = AddMultibodyPlantSceneGraph(builder, 0.0005) parser = Parser(plant) model = parser.AddModelFromFile(sdf_path) # Set initial pose floating above the ground. plant.SetDefaultFreeBodyPose(plant.GetBodyByName("body"), RigidTransform(p=[0., 0., 0.25])) if args.welded: plant.WeldFrames(plant.world_frame(), plant.GetBodyByName("body").body_frame()) else: add_ground(plant) plant.Finalize() controller = builder.AddSystem(ServoController(plant, config_dict)) # Fixed control-rate controller with a low pass filter on its torque output. zoh = builder.AddSystem( ZeroOrderHold(period_sec=0.001, vector_size=controller.n_servos)) filter = builder.AddSystem( FirstOrderLowPassFilter(time_constant=0.02, size=controller.n_servos)) builder.Connect(plant.get_state_output_port(), controller.get_input_port(1)) builder.Connect(controller.get_output_port(0), zoh.get_input_port(0)) builder.Connect(zoh.get_output_port(0), filter.get_input_port(0)) builder.Connect(filter.get_output_port(0), plant.get_actuation_input_port()) return plant, scene_graph, controller
def grasp_poses_example(): builder = DiagramBuilder() plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step=0.0) parser = Parser(plant, scene_graph) grasp = parser.AddModelFromFile( FindResourceOrThrow( "drake/manipulation/models/wsg_50_description/sdf/schunk_wsg_50_no_tip.sdf" ), "grasp") brick = parser.AddModelFromFile( FindResourceOrThrow( "drake/examples/manipulation_station/models/061_foam_brick.sdf")) plant.Finalize() meshcat = ConnectMeshcatVisualizer(builder, scene_graph) diagram = builder.Build() context = diagram.CreateDefaultContext() plant_context = plant.GetMyContextFromRoot(context) B_O = plant.GetBodyByName("base_link", brick) X_WO = plant.EvalBodyPoseInWorld(plant_context, B_O) B_Ggrasp = plant.GetBodyByName("body", grasp) p_GgraspO = [0, 0.12, 0] R_GgraspO = RotationMatrix.MakeXRotation(np.pi / 2.0).multiply( RotationMatrix.MakeZRotation(np.pi / 2.0)) X_GgraspO = RigidTransform(R_GgraspO, p_GgraspO) X_OGgrasp = X_GgraspO.inverse() X_WGgrasp = X_WO.multiply(X_OGgrasp) plant.SetFreBodyPose(plant_context, B_Ggrasp, X_WGgrasp) # Open the fingers, too plant.GetJointByName("left_finger_sliding_joint", grasp).set_translation(plant_context, -0.054) plant.GetJointByName("right_finger_sliding_joint", grasp).set_translation(plant_context, 0.054) meshcat.load() diagram.Publish(context)
def main(): builder = DiagramBuilder() plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step=0.0) path = subprocess.check_output([ 'venv/share/drake/common/resource_tool', '-print_resource_path', 'drake/examples/multibody/cart_pole/cart_pole.sdf', ]).strip() Parser(plant).AddModelFromFile(path) plant.Finalize() # Add to visualizer. DrakeVisualizer.AddToBuilder(builder, scene_graph) # Add controller. controller = builder.AddSystem(BalancingLQR(plant)) builder.Connect(plant.get_state_output_port(), controller.get_input_port(0)) builder.Connect(controller.get_output_port(0), plant.get_actuation_input_port()) diagram = builder.Build() # Set up a simulator to run this diagram. simulator = Simulator(diagram) simulator.set_target_realtime_rate(1.0) context = simulator.get_mutable_context() # Simulate. duration = 8.0 for i in range(5): initial_state = UPRIGHT_STATE + 0.1 * np.random.randn(4) print(f"Iteration: {i}. Initial: {initial_state}") context.SetContinuousState(initial_state) context.SetTime(0.0) simulator.Initialize() simulator.AdvanceTo(duration)
from pydrake.all import (AddMultibodyPlantSceneGraph, DiagramBuilder, Parser, Simulator) from underactuated import FindResource, PlanarSceneGraphVisualizer # Set up a block diagram with the robot (dynamics) and a visualization block. builder = DiagramBuilder() plant, scene_graph = AddMultibodyPlantSceneGraph(builder) # Load the double pendulum from Universal Robot Description Format parser = Parser(plant, scene_graph) parser.AddModelFromFile(FindResource("double_pendulum/double_pendulum.urdf")) plant.Finalize() builder.ExportInput(plant.get_actuation_input_port()) visualizer = builder.AddSystem(PlanarSceneGraphVisualizer(scene_graph, xlim=[-2.8, 2.8], ylim=[-2.8, 2.8])) builder.Connect(scene_graph.get_pose_bundle_output_port(), visualizer.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) # Set the initial conditions context = simulator.get_mutable_context() # state is (theta1, theta2, theta1dot, theta2dot) context.SetContinuousState([1., 1., 0., 0.])
class TimeSteppingMultibodyPlant(): """ """ def __init__(self, file=None, terrain=FlatTerrain(), dlevel=1): """ Initialize TimeSteppingMultibodyPlant with a model from a file and an arbitrary terrain geometry. Initialization also welds the first frame in the MultibodyPlant to the world frame """ self.builder = DiagramBuilder() self.multibody, self.scene_graph = AddMultibodyPlantSceneGraph( self.builder, 0.001) # Store the terrain self.terrain = terrain self._dlevel = dlevel # Build the MultibodyPlant from the file, if one exists self.model_index = [] if file is not None: # Parse the file self.model_index = Parser(self.multibody).AddModelFromFile( FindResource(file)) # Initialize the collision data self.collision_frames = [] self.collision_poses = [] self.collision_radius = [] def Finalize(self): """ Cements the topology of the MultibodyPlant and identifies all available collision geometries. """ # Finalize the underlying plant model self.multibody.Finalize() # Idenify and store collision geometries self.__store_collision_geometries() def num_contacts(self): """ returns the number of contact points""" return len(self.collision_frames) def num_friction(self): """ returns the number of friction components""" return 4 * self.dlevel * self.num_contacts() def GetNormalDistances(self, context): """ Returns an array of signed distances between the contact geometries and the terrain, given the current system context Arguments: context: a pyDrake MultibodyPlant context Return values: distances: a numpy array of signed distance values """ qtype = self.multibody.GetPositions(context).dtype nCollisions = len(self.collision_frames) distances = np.zeros((nCollisions, ), dtype=qtype) for n in range(0, nCollisions): # Transform collision frames to world coordinates collision_pt = self.multibody.CalcPointsPositions( context, self.collision_frames[n], self.collision_poses[n].translation(), self.multibody.world_frame()) # Squeeze collision point (necessary for AutoDiff plants) collision_pt = np.squeeze(collision_pt) # Calc nearest point on terrain in world coordinates terrain_pt = self.terrain.nearest_point(collision_pt) # Calc normal distance to terrain terrain_frame = self.terrain.local_frame(terrain_pt) normal = terrain_frame[0, :] distances[n] = normal.dot(collision_pt - terrain_pt) - self.collision_radius[n] # Return the distances as a single array return distances def GetContactJacobians(self, context): """ Returns a tuple of numpy arrays representing the normal and tangential contact Jacobians evaluated at each contact point Arguments: context: a pyDrake MultibodyPlant context Return Values (Jn, Jt): the tuple of contact Jacobians. Jn represents the normal components and Jt the tangential components """ qtype = self.multibody.GetPositions(context).dtype numN = self.num_contacts() numT = int(self.num_friction() / numN) D = self.friction_discretization_matrix().transpose() Jn = np.zeros((numN, self.multibody.num_velocities()), dtype=qtype) Jt = np.zeros((numN * numT, self.multibody.num_velocities()), dtype=qtype) for n in range(0, numN): # Transform collision frames to world coordinates collision_pt = self.multibody.CalcPointsPositions( context, self.collision_frames[n], self.collision_poses[n].translation(), self.multibody.world_frame()) # Calc nearest point on terrain in world coordinates terrain_pt = self.terrain.nearest_point(collision_pt) # Calc normal distance to terrain terrain_frame = self.terrain.local_frame(terrain_pt) normal, tangent = np.split(terrain_frame, [1], axis=0) # Discretize to the chosen level Dtangent = D.dot(tangent) # Get the contact point Jacobian J = self.multibody.CalcJacobianTranslationalVelocity( context, JacobianWrtVariable.kV, self.collision_frames[n], self.collision_poses[n].translation(), self.multibody.world_frame(), self.multibody.world_frame()) # Calc contact Jacobians Jn[n, :] = normal.dot(J) Jt[n * numT:(n + 1) * numT, :] = Dtangent.dot(J) # Return the Jacobians as a tuple of np arrays return (Jn, Jt) def GetFrictionCoefficients(self, context): """ Return friction coefficients for nearest point on terrain Arguments: context: the current MultibodyPlant context Return Values: friction_coeff: list of friction coefficients """ friction_coeff = [] for frame, pose in zip(self.collision_frames, self.collision_poses): # Transform collision frames to world coordinates collision_pt = self.multibody.CalcPointsPositions( context, frame, pose.translation(), self.multibody.world_frame()) # Calc nearest point on terrain in world coordiantes terrain_pt = self.terrain.nearest_point(collision_pt) friction_coeff.append(self.terrain.get_friction(terrain_pt)) # Return list of friction coefficients return friction_coeff def getTerrainPointsAndFrames(self, context): """ Return the nearest points on the terrain and the local coordinate frame Arguments: context: current MultibodyPlant context Return Values: terrain_pts: a 3xN array of points on the terrain terrain_frames: a 3x3xN array, specifying the local frame of the terrain """ terrain_pts = [] terrain_frames = [] for frame, pose in zip(self.collision_frames, self.collision_poses): # Calc collision point collision_pt = self.multibody.CalcPointsPositions( context, frame, pose.translation(), self.multibody.world_frame()) # Calc nearest point on terrain in world coordinates terrain_pt = self.terrain.nearest_point(collision_pt) terrain_pts.append(terrain_pt) # Calc local coordinate frame terrain_frames.append(self.terrain.local_frame(terrain_pt)) return (terrain_pts, terrain_frames) def toAutoDiffXd(self): """Covert the MultibodyPlant to use AutoDiffXd instead of Float""" # Create a new TimeSteppingMultibodyPlant model copy_ad = TimeSteppingMultibodyPlant(file=None, terrain=self.terrain, dlevel=self._dlevel) # Instantiate the plant as the Autodiff version copy_ad.multibody = self.multibody.ToAutoDiffXd() copy_ad.scene_graph = self.scene_graph.ToAutoDiffXd() copy_ad.model_index = self.model_index # Store the collision frames to finalize the model copy_ad.__store_collision_geometries() return copy_ad def set_discretization_level(self, dlevel=0): """Set the friction discretization level. The default is 0""" self._dlevel = dlevel def __store_collision_geometries(self): """Identifies the collision geometries in the model and stores their parent frame and pose in parent frame in lists""" # Create a diagram and a scene graph inspector = self.scene_graph.model_inspector() # Locate collision geometries and contact points body_inds = self.multibody.GetBodyIndices(self.model_index) # Get the collision frames for each body in the model for body_ind in body_inds: body = self.multibody.get_body(body_ind) collision_ids = self.multibody.GetCollisionGeometriesForBody(body) for id in collision_ids: # get and store the collision geometry frames frame_name = inspector.GetName( inspector.GetFrameId(id)).split("::") self.collision_frames.append( self.multibody.GetFrameByName(frame_name[-1])) self.collision_poses.append(inspector.GetPoseInFrame(id)) # Check for a spherical geometry geoms = inspector.GetGeometries(inspector.GetFrameId(id), Role.kProximity) shape = inspector.GetShape(geoms[0]) if type(shape) is Sphere: self.collision_radius.append(shape.radius()) else: self.collision_radius.append(0.) def __discretize_friction(self, normal, tangent): """ Rotates the terrain tangent vectors to discretize the friction cone Arguments: normal: The terrain normal direction, (1x3) numpy array tangent: The terrain tangent directions, (2x3) numpy array Return Values: all_tangents: The discretized friction vectors, (2nx3) numpy array This method is now deprecated """ # Reflect the current friction basis tangent = np.concatenate((tangent, -tangent), axis=0) all_tangents = np.zeros((4 * (self._dlevel), tangent.shape[1])) all_tangents[0:4, :] = tangent # Rotate the tangent basis around the normal vector for n in range(1, self._dlevel): # Create an angle-axis representation of rotation R = RotationMatrix(theta_lambda=AngleAxis( angle=n * pi / (2 * (self._dlevel)), axis=normal)) # Apply the rotation matrix all_tangents[n * 4:(n + 1) * 4, :] = R.multiply( tangent.transpose()).transpose() return all_tangents def simulate(self, h, x0, u=None, N=1): # Initialize arrays nx = x0.shape[0] x = np.zeros(shape=(nx, N)) x[:, 0] = x0 t = np.zeros(shape=(N, )) nf = 1 if u is None: B = self.multibody.MakeAcutatorMatrix() u = np.zeros(shape=(B.shape[1], N)) context = self.multibody.CreateDefaultContext() Jn, Jt = self.GetContactJacobians(context) f = np.zeros(shape=(Jn.shape[0] + Jt.shape[0], N)) # Integration loop for n in range(0, N - 1): f[:, n] = self.contact_impulse(h, x[:, n], u[:, n]) x[:, n + 1] = self.integrate(h, x[:, n], u[:, n], f[:, n]) t[n + 1] = t[n] + h f[:, n] = f[:, n] / h return (t, x, f) def integrate(self, h, x, u, f): # Get the configuration and the velocity q, dq = np.split(x, 2) # Estimate the next configuration, assuming constant velocity qhat = q + h * dq # Set the context context = self.multibody.CreateDefaultContext() self.multibody.SetPositions(context, qhat) v = self.multibody.MapQDotToVelocity(context, dq) self.multibody.SetVelocities(context, v) # Get the current system properties M = self.multibody.CalcMassMatrixViaInverseDynamics(context) C = self.multibody.CalcBiasTerm(context) G = self.multibody.CalcGravityGeneralizedForces(context) B = self.multibody.MakeActuationMatrix() Jn, Jt = self.GetContactJacobians(context) J = np.vstack((Jn, Jt)) # Calculate the next state b = h * (B.dot(u) - C.dot(dq) + G) + J.transpose().dot(f) v = np.linalg.solve(M, b) dq += v q += h * dq # Collect the configuration and velocity into a state vector return np.concatenate((q, dq), axis=0) def contact_impulse(self, h, x, u): # Get the configuration and generalized velocity q, dq = np.split(x, 2) # Estimate the configuration at the next time step qhat = q + h * dq # Get the system parameters context = self.multibody.CreateDefaultContext() self.multibody.SetPositions(context, q) v = self.multibody.MapQDotToVelocity(context, dq) self.multibody.SetVelocities(context, v) M = self.multibody.CalcMassMatrixViaInverseDynamics(context) C = self.multibody.CalcBiasTerm(context) G = self.multibody.CalcGravityGeneralizedForces(context) B = self.multibody.MakeActuationMatrix() tau = B.dot(u) - C + G Jn, Jt = self.GetContactJacobians(context) phi = self.GetNormalDistances(context) alpha = Jn.dot(qhat) - phi # Calculate the force size from the contact Jacobian numT = Jt.shape[0] numN = Jn.shape[0] S = numT + 2 * numN # Initialize LCP parameters P = np.zeros(shape=(S, S), dtype=float) w = np.zeros(shape=(numN, S)) numF = int(numT / numN) for n in range(0, numN): w[n, n * numF + numN:numN + (n + 1) * numF] = 1 # Construct LCP matrix J = np.vstack((Jn, Jt)) JM = J.dot(np.linalg.inv(M)) P[0:numN + numT, 0:numN + numT] = JM.dot(J.transpose()) P[:, numN + numT:] = w.transpose() P[numN + numT:, :] = -w P[numN + numT:, 0:numN] = np.diag(self.GetFrictionCoefficients(context)) #Construct LCP bias vector z = np.zeros(shape=(S, ), dtype=float) z[0:numN + numT] = h * JM.dot(tau) + J.dot(dq) #z[0:numN] += (Jn.dot(q) - alpha)/h z[0:numN] += phi / h # Solve the LCP for the reaction impluses f, status = solve_lcp(P, z) if f is None: return np.zeros(shape=(numN + numT, )) else: # Strip the slack variables from the LCP solution return f[0:numN + numT] def get_multibody(self): return self.multibody def joint_limit_jacobian(self): """ Returns a matrix for mapping the positive-only joint limit torques to """ # Create the Jacobian as if all joints were limited nV = self.multibody.num_velocities() I = np.eye(nV) # Check for infinite limits qhigh = self.multibody.GetPositionUpperLimits() qlow = self.multibody.GetPositionLowerLimits() # Assert two sided limits low_inf = np.isinf(qlow) assert all(low_inf == np.isinf(qhigh)) # Make the joint limit Jacobian if not all(low_inf): # Remove floating base limits floating = self.multibody.GetFloatingBaseBodies() floating_pos = [] while len(floating) > 0: body = self.multibody.get_body(floating.pop()) if body.has_quaternion_dofs(): floating_pos.append(body.floating_positions_start()) # Remove entries corresponding to the extra dof from quaternions low_inf = np.delete(low_inf, floating_pos) low_inf = np.squeeze(low_inf) # Zero-out the entries that don't correspond to joint limits I[low_inf, low_inf] = 0 # Remove the columns that don't correspond to joint limits I = np.delete(I, low_inf, axis=1) return np.concatenate([I, -I], axis=1) else: return None def get_contact_points(self, context): """ Returns a list of positions of contact points expressed in world coordinates given the system context. """ contact_pts = [] for frame, pose in zip(self.collision_frames, self.collision_poses): contact_pts.append( self.multibody.CalcPointsPositions( context, frame, pose.translation(), self.multibody.world_frame())) return contact_pts def resolve_contact_forces_in_world(self, context, forces): """ Transform non-negative discretized force components used in complementarity constraints into 3-vectors in world coordinates Returns a list of (3,) numpy arrays """ # First remove the discretization forces = self.resolve_forces(forces) # Reorganize forces from (Normal, Tangential) to a list force_list = [] for n in range(0, self.num_contacts()): force_list.append(forces[[ n, self.num_contacts() + 2 * n, self.num_contacts() + 2 * n + 1 ]]) # Transform the forces into world coordinates using the terrain frames _, frames = self.getTerrainPointsAndFrames(context) world_forces = [] for force, frame in zip(force_list, frames): world_forces.append(frame.dot(force)) # Return a list of forces in world coordinates return world_forces def resolve_limit_forces(self, jl_forces): """ Combine positive and negative components of joint limit torques Arguments: jl_forces: (2n, m) numpy array Return values: (n, m) numpy array """ JL = self.joint_limit_jacobian() has_limits = np.sum(abs(JL), axis=1) > 0 f_jl = JL.dot(jl_forces) if f_jl.ndim > 1: return f_jl[has_limits, :] else: return f_jl[has_limits] def duplicator_matrix(self): """Returns a matrix of 1s and 0s for combining friction forces or duplicating sliding velocities""" numN = self.num_contacts() numT = self.num_friction() w = np.zeros((numN, numT)) nD = int(numT / numN) for n in range(numN): w[n, n * nD:(n + 1) * nD] = 1 return w def friction_discretization_matrix(self): """ Make a matrix for converting discretized friction into a single vector""" n = 4 * self.dlevel theta = np.linspace(0, n - 1, n) * 2 * np.pi / n D = np.vstack((np.cos(theta), np.sin(theta))) # Threshold out small values D[np.abs(D) < 1e-10] = 0 return D def resolve_forces(self, forces): """ Convert discretized friction & normal forces into a non-discretized 3-vector""" numN = self.num_contacts() n = 4 * self.dlevel fN = forces[0:numN, :] fT = forces[numN:numN * (n + 1), :] D_ = self.friction_discretization_matrix() D = np.zeros((2 * numN, n * numN)) for k in range(numN): D[2 * k:2 * k + 2, k * n:(k + 1) * n] = D_ ff = D.dot(fT) return np.concatenate((fN, ff), axis=0) def static_controller(self, qref, verbose=False): """ Generates a controller to maintain a static pose Arguments: qref: (N,) numpy array, the static pose to be maintained Return Values: u: (M,) numpy array, actuations to best achieve static pose f: (C,) numpy array, associated normal reaction forces static_controller generates the actuations and reaction forces assuming the velocity and accelerations are zero. Thus, the equation to solve is: N(q) = B*u + J*f where N is a vector of gravitational and conservative generalized forces, B is the actuation selection matrix, and J is the contact-Jacobian transpose. Currently, static_controller only considers the effects of the normal forces. Frictional forces are not yet supported """ #TODO: Solve for friction forces as well # Check inputs if qref.ndim > 1 or qref.shape[0] != self.multibody.num_positions(): raise ValueError( f"Reference position mut be ({self.multibody.num_positions(),},) array" ) # Set the context context = self.multibody.CreateDefaultContext() self.multibody.SetPositions(context, qref) # Get the necessary properties G = self.multibody.CalcGravityGeneralizedForces(context) Jn, _ = self.GetContactJacobians(context) phi = self.GetNormalDistances(context) B = self.multibody.MakeActuationMatrix() #Collect terms A = np.concatenate([B, Jn.transpose()], axis=1) # Create the mathematical program prog = MathematicalProgram() l_var = prog.NewContinuousVariables(self.num_contacts(), name="forces") u_var = prog.NewContinuousVariables(self.multibody.num_actuators(), name="controls") # Ensure dynamics approximately satisfied prog.AddL2NormCost(A=A, b=-G, vars=np.concatenate([u_var, l_var], axis=0)) # Enforce normal complementarity prog.AddBoundingBoxConstraint(np.zeros(l_var.shape), np.full(l_var.shape, np.inf), l_var) prog.AddConstraint(phi.dot(l_var) == 0) # Solve result = Solve(prog) # Check for a solution if result.is_success(): u = result.GetSolution(u_var) f = result.GetSolution(l_var) if verbose: printProgramReport(result, prog) return (u, f) else: print(f"Optimization failed. Returning zeros") if verbose: printProgramReport(result, prog) return (np.zeros(u_var.shape), np.zeros(l_var.shape)) @property def dlevel(self): return self._dlevel @dlevel.setter def dlevel(self, val): """Check that the value of dlevel is a positive integer""" if type(val) is int and val > 0: self._dlevel = val else: raise ValueError("dlevel must be a positive integer")
def make_box_flipup(generator, observations="state", meshcat=None, time_limit=10): builder = DiagramBuilder() plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step=0.001) # TODO(russt): randomize parameters. box = AddPlanarBinAndSimpleBox(plant) finger = AddPointFinger(plant) plant.Finalize() plant.set_name("plant") SetTransparency(scene_graph, alpha=0.5, source_id=plant.get_source_id()) controller_plant = MultibodyPlant(time_step=0.005) AddPointFinger(controller_plant) if meshcat: MeshcatVisualizerCpp.AddToBuilder(builder, scene_graph, meshcat) meshcat.Set2dRenderMode(xmin=-.35, xmax=.35, ymin=-0.1, ymax=0.3) ContactVisualizer.AddToBuilder( builder, plant, meshcat, ContactVisualizerParams(radius=0.005, newtons_per_meter=40.0)) # Use the controller plant to visualize the set point geometry. controller_scene_graph = builder.AddSystem(SceneGraph()) controller_plant.RegisterAsSourceForSceneGraph(controller_scene_graph) SetColor(controller_scene_graph, color=[1.0, 165.0 / 255, 0.0, 1.0], source_id=controller_plant.get_source_id()) controller_vis = MeshcatVisualizerCpp.AddToBuilder( builder, controller_scene_graph, meshcat, MeshcatVisualizerParams(prefix="controller")) controller_vis.set_name("controller meshcat") controller_plant.Finalize() # Stiffness control. (For a point finger with unit mass, the # InverseDynamicsController is identical) N = controller_plant.num_positions() kp = [100] * N ki = [1] * N kd = [2 * np.sqrt(kp[0])] * N controller = builder.AddSystem( InverseDynamicsController(controller_plant, kp, ki, kd, False)) builder.Connect(plant.get_state_output_port(finger), controller.get_input_port_estimated_state()) actions = builder.AddSystem(PassThrough(N)) positions_to_state = builder.AddSystem(Multiplexer([N, N])) builder.Connect(actions.get_output_port(), positions_to_state.get_input_port(0)) zeros = builder.AddSystem(ConstantVectorSource([0] * N)) builder.Connect(zeros.get_output_port(), positions_to_state.get_input_port(1)) builder.Connect(positions_to_state.get_output_port(), controller.get_input_port_desired_state()) builder.Connect(controller.get_output_port(), plant.get_actuation_input_port()) if meshcat: positions_to_poses = builder.AddSystem( MultibodyPositionToGeometryPose(controller_plant)) builder.Connect( positions_to_poses.get_output_port(), controller_scene_graph.get_source_pose_port( controller_plant.get_source_id())) builder.ExportInput(actions.get_input_port(), "actions") if observations == "state": builder.ExportOutput(plant.get_state_output_port(), "observations") # TODO(russt): Add 'time', and 'keypoints' else: raise ValueError("observations must be one of ['state']") class RewardSystem(LeafSystem): def __init__(self): LeafSystem.__init__(self) self.DeclareVectorInputPort("box_state", 6) self.DeclareVectorInputPort("finger_state", 4) self.DeclareVectorInputPort("actions", 2) self.DeclareVectorOutputPort("reward", 1, self.CalcReward) def CalcReward(self, context, output): box_state = self.get_input_port(0).Eval(context) finger_state = self.get_input_port(1).Eval(context) actions = self.get_input_port(2).Eval(context) angle_from_vertical = (box_state[2] % np.pi) - np.pi / 2 cost = 2 * angle_from_vertical**2 # box angle cost += 0.1 * box_state[5]**2 # box velocity effort = actions - finger_state[:2] cost += 0.1 * effort.dot(effort) # effort # finger velocity cost += 0.1 * finger_state[2:].dot(finger_state[2:]) # Add 10 to make rewards positive (to avoid rewarding simulator # crashes). output[0] = 10 - cost reward = builder.AddSystem(RewardSystem()) builder.Connect(plant.get_state_output_port(box), reward.get_input_port(0)) builder.Connect(plant.get_state_output_port(finger), reward.get_input_port(1)) builder.Connect(actions.get_output_port(), reward.get_input_port(2)) builder.ExportOutput(reward.get_output_port(), "reward") # Set random state distributions. uniform_random = Variable(name="uniform_random", type=Variable.Type.RANDOM_UNIFORM) box_joint = plant.GetJointByName("box") x, y = box_joint.get_default_translation() box_joint.set_random_pose_distribution([.2 * uniform_random - .1 + x, y], 0) diagram = builder.Build() simulator = Simulator(diagram) # Termination conditions: def monitor(context): if context.get_time() > time_limit: return EventStatus.ReachedTermination(diagram, "time limit") return EventStatus.Succeeded() simulator.set_monitor(monitor) return simulator
""" # think of the builder as the construction site of our block diagram builder = DiagramBuilder() # instantiate the vibrating pendulum and the scene graph # the scene graph is a container for the geometries of all the physical systems in our diagram vibrating_pendulum, scene_graph = AddMultibodyPlantSceneGraph( builder, time_step=0. # discrete update period , set to zero since system is continuous ) # parse the urdf and populate the vibrating pendulum urdf_path = FindResource('models/vibrating_pendulum.urdf') Parser(vibrating_pendulum).AddModelFromFile(urdf_path) vibrating_pendulum.Finalize() """## Write the Controller In this section we define two controllers: 1. An inner controller that makes the base oscillate with the harmonic motion. We wrote this for you. 2. The outer controller to make the pendulum spin at constant velocity. You will write part of this. The final diagram will have the following structure: ![figure](https://raw.githubusercontent.com/RussTedrake/underactuated/master/figures/exercises/vibrating_pendulum.jpg) """ # this controller enforces the harmonic motion to the base class InnerController(VectorSystem): def __init__(self, vibrating_pendulum):
def perform_iou_testing(model_file, test_specific_temp_directory, pose_directory): random_poses = {} # Read camera translation calculated and applied on gazebo # we read the random positions file as it contains everything: with open( test_specific_temp_directory + "/pics/" + pose_directory + "/poses.txt", "r") as datafile: for line in datafile: if line.startswith("Translation:"): line_split = line.split(" ") # we make the value negative since gazebo moved the robot # and in drakewe move the camera trans_x = float(line_split[1]) trans_y = float(line_split[2]) trans_z = float(line_split[3]) elif line.startswith("Scaling:"): line_split = line.split(" ") scaling = float(line_split[1]) else: line_split = line.split(" ") if line_split[1] == "nan": random_poses[line_split[0][:-1]] = 0 else: random_poses[line_split[0][:-1]] = float(line_split[1]) builder = DiagramBuilder() plant, scene_graph = AddMultibodyPlantSceneGraph(builder, 0.0) parser = Parser(plant) model = make_parser(plant).AddModelFromFile(model_file) model_bodies = me.get_bodies(plant, {model}) frame_W = plant.world_frame() frame_B = model_bodies[0].body_frame() if len(plant.GetBodiesWeldedTo(plant.world_body())) < 2: plant.WeldFrames(frame_W, frame_B, X_PC=plant.GetDefaultFreeBodyPose(frame_B.body())) # Creating cameras: renderer_name = "renderer" scene_graph.AddRenderer(renderer_name, MakeRenderEngineVtk(RenderEngineVtkParams())) # N.B. These properties are chosen arbitrarily. depth_camera = DepthRenderCamera( RenderCameraCore( renderer_name, CameraInfo( width=960, height=540, focal_x=831.382036787, focal_y=831.382036787, center_x=480, center_y=270, ), ClippingRange(0.01, 10.0), RigidTransform(), ), DepthRange(0.01, 10.0), ) world_id = plant.GetBodyFrameIdOrThrow(plant.world_body().index()) # Creating perspective cam X_WB = xyz_rpy_deg( [ 1.6 / scaling + trans_x, -1.6 / scaling + trans_y, 1.2 / scaling + trans_z ], [-120, 0, 45], ) sensor_perspective = create_camera(builder, world_id, X_WB, depth_camera, scene_graph) # Creating top cam X_WB = xyz_rpy_deg([0 + trans_x, 0 + trans_y, 2.2 / scaling + trans_z], [-180, 0, -90]) sensor_top = create_camera(builder, world_id, X_WB, depth_camera, scene_graph) # Creating front cam X_WB = xyz_rpy_deg([2.2 / scaling + trans_x, 0 + trans_y, 0 + trans_z], [-90, 0, 90]) sensor_front = create_camera(builder, world_id, X_WB, depth_camera, scene_graph) # Creating side cam X_WB = xyz_rpy_deg([0 + trans_x, 2.2 / scaling + trans_y, 0 + trans_z], [-90, 0, 180]) sensor_side = create_camera(builder, world_id, X_WB, depth_camera, scene_graph) # Creating back cam X_WB = xyz_rpy_deg([-2.2 / scaling + trans_x, 0 + trans_y, 0 + trans_z], [-90, 0, -90]) sensor_back = create_camera(builder, world_id, X_WB, depth_camera, scene_graph) DrakeVisualizer.AddToBuilder(builder, scene_graph) # Remove gravity to avoid extra movements of the model when running the simulation plant.gravity_field().set_gravity_vector( np.array([0, 0, 0], dtype=np.float64)) # Switch off collisions to avoid problems with random positions collision_filter_manager = scene_graph.collision_filter_manager() model_inspector = scene_graph.model_inspector() geometry_ids = GeometrySet(model_inspector.GetAllGeometryIds()) collision_filter_manager.Apply( CollisionFilterDeclaration().ExcludeWithin(geometry_ids)) plant.Finalize() diagram = builder.Build() simulator = Simulator(diagram) simulator.Initialize() dofs = plant.num_actuated_dofs() if dofs != plant.num_positions(): raise ValueError( "Error on converted model: Num positions is not equal to num actuated dofs." ) if pose_directory == "random_pose": joint_positions = [0] * dofs for joint_name, pose in random_poses.items(): # check if NaN if pose != pose: pose = 0 # drake will add '_joint' when there's a name collision if plant.HasJointNamed(joint_name): joint = plant.GetJointByName(joint_name) else: joint = plant.GetJointByName(joint_name + "_joint") joint_positions[joint.position_start()] = pose sim_plant_context = plant.GetMyContextFromRoot( simulator.get_mutable_context()) plant.get_actuation_input_port(model).FixValue(sim_plant_context, np.zeros((dofs, 1))) plant.SetPositions(sim_plant_context, model, joint_positions) simulator.AdvanceTo(1) generate_images_and_iou(simulator, sensor_perspective, test_specific_temp_directory, pose_directory, 1) generate_images_and_iou(simulator, sensor_top, test_specific_temp_directory, pose_directory, 2) generate_images_and_iou(simulator, sensor_front, test_specific_temp_directory, pose_directory, 3) generate_images_and_iou(simulator, sensor_side, test_specific_temp_directory, pose_directory, 4) generate_images_and_iou(simulator, sensor_back, test_specific_temp_directory, pose_directory, 5)
def build_mbp(seed=0, verts_geom=False, convex_collision_geom=True): # Make some random lumpy objects trimeshes = [] np.random.seed(42) for k in range(3): # Make a small random number of triangles and chull it # to get a lumpy object mesh = trimesh.creation.random_soup(5) mesh = trimesh.convex.convex_hull(mesh) trimeshes.append(mesh) # Create Drake geometry from those objects by adding a small # sphere at each vertex sphere_rad = 0.05 cmap = plt.cm.get_cmap('jet') builder = DiagramBuilder() mbp, scene_graph = AddMultibodyPlantSceneGraph( builder, MultibodyPlant(time_step=0.001)) # Add ground friction = CoulombFriction(0.9, 0.8) g = pydrake_geom.Box(100., 100., 0.5) tf = RigidTransform(p=[0., 0., -0.25]) mbp.RegisterVisualGeometry(body=mbp.world_body(), X_BG=tf, shape=g, name="ground", diffuse_color=[1.0, 1.0, 1.0, 1.0]) mbp.RegisterCollisionGeometry(body=mbp.world_body(), X_BG=tf, shape=g, name="ground", coulomb_friction=friction) for i, mesh in enumerate(trimeshes): inertia = SpatialInertia(mass=1.0, p_PScm_E=np.zeros(3), G_SP_E=UnitInertia(0.01, 0.01, 0.01)) body = mbp.AddRigidBody(name="body_%d" % i, M_BBo_B=inertia) color = cmap(np.random.random()) if verts_geom: for j, vert in enumerate(mesh.vertices): g = pydrake_geom.Sphere(radius=sphere_rad) tf = RigidTransform(p=vert) mbp.RegisterVisualGeometry(body=body, X_BG=tf, shape=g, name="body_%d_color_%d" % (i, j), diffuse_color=color) mbp.RegisterCollisionGeometry(body=body, X_BG=tf, shape=g, name="body_%d_collision_%d" % (i, j), coulomb_friction=friction) # And add mesh itself for vis path = "/tmp/part_%d.obj" % i trimesh.exchange.export.export_mesh(mesh, path) g = pydrake_geom.Convex(path) mbp.RegisterVisualGeometry(body=body, X_BG=RigidTransform(), shape=g, name="body_%d_base" % i, diffuse_color=color) if convex_collision_geom: mbp.RegisterCollisionGeometry(body=body, X_BG=RigidTransform(), shape=g, name="body_%d_base_col" % i, coulomb_friction=friction) mbp.SetDefaultFreeBodyPose(body, RigidTransform(p=[i % 3, i / 3., 1.])) mbp.Finalize() return builder, mbp, scene_graph
x_star = [0, np.pi, 0, 0] # weight matrices for the lqr controller Q = np.eye(4) R = np.eye(1) """Then we construct the block diagram with the cart-pole in closed loop with the LQR controller.""" # start construction site of our block diagram builder = DiagramBuilder() # instantiate the cart-pole and the scene graph cartpole, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step=0.0) urdf_path = FindResource('models/undamped_cartpole.urdf') Parser(cartpole).AddModelFromFile(urdf_path) cartpole.Finalize() # set the operating point (vertical unstable equilibrium) context = cartpole.CreateDefaultContext() context.get_mutable_continuous_state_vector().SetFromVector(x_star) # fix the input port to zero and get its index for the lqr function cartpole.get_actuation_input_port().FixValue(context, [0]) input_i = cartpole.get_actuation_input_port().get_index() # synthesize lqr controller directly from # the nonlinear system and the operating point lqr = LinearQuadraticRegulator(cartpole, context, Q, R, input_port_index=input_i) lqr = builder.AddSystem(lqr) # the following two lines are not needed here...
class iiwa_sys(): def __init__(self, builder, dt=5e-4, N=150, params=None, trj_decay=0.7, x_w_cov=1e-5, door_angle_ref=1.0, visualize=False): self.plant_derivs = MultibodyPlant(time_step=dt) parser = Parser(self.plant_derivs) self.derivs_iiwa, _, _ = self.add_models(self.plant_derivs, parser, params=params) self.plant_derivs.Finalize() self.plant_derivs_context = self.plant_derivs.CreateDefaultContext() self.plant_derivs.get_actuation_input_port().FixValue( self.plant_derivs_context, [0., 0., 0., 0., 0., 0., 0.]) null_force = BasicVector([0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]) self.plant_derivs.GetInputPort("applied_generalized_force").FixValue( self.plant_derivs_context, null_force) self.plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step=dt) parser = Parser(self.plant, scene_graph) self.iiwa, self.hinge, self.bushing = self.add_models(self.plant, parser, params=params) self.plant.Finalize( ) # Finalize will assign ports for compatibility w/ the scene_graph; could be cause of the issue w/ first order taylor. self.meshcat = ConnectMeshcatVisualizer(builder, scene_graph, zmq_url=zmq_url) self.sym_derivs = False # If system should use symbolic derivatives; if false, autodiff self.custom_sim = False # If rollouts should be gathered with sys.dyn() calls nq = self.plant.num_positions() nv = self.plant.num_velocities() self.n_x = nq + nv self.n_u = self.plant.num_actuators() self.n_y = self.plant.get_state_output_port(self.iiwa).size() self.N = N self.dt = dt self.decay = trj_decay self.V = 1e-2 * np.ones(self.n_y) self.W = np.concatenate((1e-7 * np.ones(nq), 1e-4 * np.ones(nv))) self.W0 = np.concatenate((1e-9 * np.ones(nq), 1e-6 * np.ones(nv))) self.x_w_cov = x_w_cov self.door_angle_ref = door_angle_ref self.q0 = np.array( [-3.12, -0.17, 0.52, -3.11, 1.22, -0.75, -1.56, 0.55]) #self.q0 = np.array([-3.12, -0.27, 0.52, -3.11, 1.22, -0.75, -1.56, 0.55]) self.x0 = np.concatenate((self.q0, np.zeros(nv))) self.door_index = None self.phi = {} def ports_init(self, context): self.plant_context = self.plant.GetMyMutableContextFromRoot(context) self.plant.SetPositionsAndVelocities(self.plant_context, self.x0) #door_angle = self.plant.GetPositionsFromArray(self.hinge, self.x0[:8]) self.door_index = self.plant.GetJointByName( 'right_door_hinge').position_start() null_force = BasicVector([0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]) self.plant.GetInputPort("applied_generalized_force").FixValue( self.plant_context, null_force) self.plant.get_actuation_input_port().FixValue( self.plant_context, [0., 0., 0., 0., 0., 0., 0.]) self.W0[self.door_index] = self.x_w_cov def add_models(self, plant, parser, params=None): iiwa = parser.AddModelFromFile( "/home/hanikevi/drake/manipulation/models/iiwa_description/sdf/iiwa14_no_collision_no_grav.sdf" ) plant.WeldFrames(plant.world_frame(), plant.GetFrameByName("iiwa_link_0")) #box = Box(10., 10., 10.) #X_WBox = RigidTransform([0, 0, -5]) #mu = 0.6 #plant.RegisterCollisionGeometry(plant.world_body(), X_WBox, box, "ground", CoulombFriction(mu, mu)) #plant.RegisterVisualGeometry(plant.world_body(), X_WBox, box, "ground", [.9, .9, .9, 1.0]) #planar_joint_frame = plant.AddFrame(FixedOffsetFrame("planar_joint_frame", plant.world_frame(), RigidTransform(RotationMatrix.MakeXRotation(np.pi/2)))) X_WCylinder = RigidTransform([-0.75, 0, 0.5]) hinge = parser.AddModelFromFile( "/home/hanikevi/drake/examples/manipulation_station/models/simple_hinge.sdf" ) plant.WeldFrames(plant.world_frame(), plant.GetFrameByName("base"), X_WCylinder) #cupboard_door_spring = plant.AddForceElement(RevoluteSpring_[float](plant.GetJointByName("right_door_hinge"), nominal_angle = -0.4, stiffness = 10)) if params is None: bushing = LinearBushingRollPitchYaw_[float]( plant.GetFrameByName("iiwa_link_7"), plant.GetFrameByName("handle"), [50, 50, 50], # Torque stiffness [2., 2., 2.], # Torque damping [5e4, 5e4, 5e4], # Linear stiffness [80, 80, 80], # Linear damping ) else: print('setting custom stiffnesses') bushing = LinearBushingRollPitchYaw_[float]( plant.GetFrameByName("iiwa_link_7"), plant.GetFrameByName("handle"), [params['k4'], params['k5'], params['k6']], # Torque stiffness [2, 2, 2], # Torque damping [params['k1'], params['k2'], params['k3']], # Linear stiffness [100, 100, 100], # Linear damping ) bushing_element = plant.AddForceElement(bushing) return iiwa, hinge, bushing def cost_stage(self, x, u): ctrl = 1e-5 * np.sum(u**2) pos = 15.0 * (x[self.door_index] - self.door_angle_ref)**2 vel = 1e-5 * np.sum(x[8:]**2) return pos + ctrl + vel def cost_final(self, x): return 50 * (1.0 * (x[self.door_index] - self.door_angle_ref)**2 + np.sum(2.5e-4 * x[8:]**2)) def get_deriv(self, x, u): self.plant_derivs.SetPositionsAndVelocities(self.plant_derivs_context, x) lin = FirstOrderTaylorApproximation( self.plant_derivs, self.plant_derivs_context, self.plant.get_actuation_input_port().get_index(), self.plant.get_state_output_port(self.iiwa).get_index()) return lin.A(), lin.B(), lin.C() def get_param_deriv(self, x, u): # Using a closed-form solution; as currently DRAKE doesn't do support autodiff on parameters for LinearBusing. # Note dC is 0 - stiffness does not affect measurements of q, v W = self.plant.world_frame() I = self.plant.GetFrameByName("iiwa_link_7") H = self.plant.GetFrameByName("handle") self.plant.SetPositionsAndVelocities(self.plant_context, x) M = self.plant.CalcMassMatrixViaInverseDynamics(self.plant_context) Jac_I = self.plant.CalcJacobianSpatialVelocity( self.plant_context, JacobianWrtVariable.kQDot, I, [0, 0, 0], W, W) Jac_H = self.plant.CalcJacobianSpatialVelocity( self.plant_context, JacobianWrtVariable.kQDot, H, [0, 0, 0], W, W) dA = np.zeros((self.n_x**2, 3)) dA_sq = np.zeros((self.n_x, self.n_x)) for param_ind in range(3): JH = np.outer(Jac_H[param_ind, :], Jac_H[param_ind, :]) JI = np.outer(Jac_I[param_ind, :], Jac_I[param_ind, :]) dA_sq[8:, :8] = self.dt * np.linalg.inv(M).dot(JH + JI) dA[:, param_ind] = deepcopy(dA_sq.ravel()) #print(np.sum(np.abs(dA), axis=0)) return dA def reset(self): x_traj_new = np.zeros((self.N + 1, self.n_x)) x_traj_new[0, :] = self.x0 + np.multiply(np.sqrt(self.W0), np.random.randn(self.n_x)) u_traj_new = np.zeros((self.N, self.n_u)) #self.plant_context.SetDiscreteState(x_traj_new[0,:]) self.plant.SetPositionsAndVelocities(self.plant_context, x_traj_new[0, :]) return x_traj_new, u_traj_new def rollout(self): self.u_trj = np.random.randn(self.N, self.n_u) * 0.001 self.x_trj, _ = self.reset() for i in range(self.N): self.x_trj[i + 1, :] = self.dyn(self.x_trj[i, :], self.u_trj[i], noise=True) def dyn(self, x, u, phi=None, noise=False): x_next = self.plant.AllocateDiscreteVariables() self.plant.SetPositionsAndVelocities(self.plant_context, x) self.plant.get_actuation_input_port(self.iiwa).FixValue( self.plant_context, u) self.plant.CalcDiscreteVariableUpdates(self.plant_context, x_next) #print(x_next.get_mutable_vector().get_value()) x_new = x_next.get_mutable_vector().get_value() if noise: noise = np.multiply(np.sqrt(self.W), np.random.randn(self.n_x)) x_new += noise return x_new def obs(self, x, mode=None, noise=False, phi=None): y = self.plant.get_state_output_port(self.iiwa).Eval( self.plant_context) #print(self.bushing.CalcBushingSpatialForceOnFrameA(self.plant_context).translational()) if noise: y += np.multiply(np.sqrt(self.V), np.random.randn(self.n_y)) return y def cost(self, x_trj=None, u_trj=None): cost_trj = 0.0 if x_trj is None: for i in range(self.N): cost_trj += self.cost_stage(self.x_trj[i, :], self.u_trj[i, :]) cost_trj += self.cost_final(self.sys.plant, self.x_trj[-1, :]) else: for i in range(self.N): cost_trj += self.cost_stage(x_trj[i, :], u_trj[i, :]) cost_trj += self.cost_final(x_trj[-1, :]) return cost_trj
from pydrake.common.eigen_geometry import Isometry3 from pydrake.all import AddMultibodyPlantSceneGraph from pydrake.multibody.tree import SpatialInertia, UnitInertia env_plant, scene_graph = AddMultibodyPlantSceneGraph(builder) for idx, object_ in enumerate(OBJECTS): sz, trans, rot = object_ body = env_plant.AddRigidBody( str(idx + 1), SpatialInertia(1.0, np.zeros(3), UnitInertia(1.0, 1.0, 1.0))) pos = Isometry3() pos.set_translation(trans) # np.array([0,1,1]) pos.set_rotation(rot) # rm([1,0,0], 90) env_plant.RegisterVisualGeometry(body, pos, Box(sz[0], sz[1], sz[2]), str(idx + 1) + 'v', np.array([1, 1, 1, 1]), scene_graph) env_plant.Finalize() plant.RegisterGeometry(scene_graph) builder.Connect(plant.get_geometry_pose_output_port(), scene_graph.get_source_pose_port(plant.source_id())) meshcat = builder.AddSystem( MeshcatVisualizer(scene_graph, zmq_url=None, open_browser=None)) builder.Connect(scene_graph.get_pose_bundle_output_port(), meshcat.get_input_port(0)) # end setup for visualization diagram = builder.Build() simulator = Simulator(diagram) simulator.set_target_realtime_rate(1.0) context = simulator.get_mutable_context()
def MakeManipulationStation(time_step=0.002, plant_setup_callback=None, camera_prefix="camera"): """ Sets up a manipulation station with the iiwa + wsg + controllers [+ cameras]. Cameras will be added to each body with a name starting with "camera". Args: time_step: the standard MultibodyPlant time step. plant_setup_callback: should be a python callable that takes one argument: `plant_setup_callback(plant)`. It will be called after the iiwa and WSG are added to the plant, but before the plant is finalized. Use this to add additional geometry. camera_prefix: Any bodies in the plant (created during the plant_setup_callback) starting with this prefix will get a camera attached. """ builder = DiagramBuilder() # Add (only) the iiwa, WSG, and cameras to the scene. plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step=time_step) iiwa = AddIiwa(plant) wsg = AddWsg(plant, iiwa) if plant_setup_callback: plant_setup_callback(plant) plant.Finalize() num_iiwa_positions = plant.num_positions(iiwa) # I need a PassThrough system so that I can export the input port. iiwa_position = builder.AddSystem(PassThrough(num_iiwa_positions)) builder.ExportInput(iiwa_position.get_input_port(), "iiwa_position") builder.ExportOutput(iiwa_position.get_output_port(), "iiwa_position_command") # Export the iiwa "state" outputs. demux = builder.AddSystem( Demultiplexer(2 * num_iiwa_positions, num_iiwa_positions)) builder.Connect(plant.get_state_output_port(iiwa), demux.get_input_port()) builder.ExportOutput(demux.get_output_port(0), "iiwa_position_measured") builder.ExportOutput(demux.get_output_port(1), "iiwa_velocity_estimated") builder.ExportOutput(plant.get_state_output_port(iiwa), "iiwa_state_estimated") # Make the plant for the iiwa controller to use. controller_plant = MultibodyPlant(time_step=time_step) controller_iiwa = AddIiwa(controller_plant) AddWsg(controller_plant, controller_iiwa, welded=True) controller_plant.Finalize() # Add the iiwa controller iiwa_controller = builder.AddSystem( InverseDynamicsController(controller_plant, kp=[100] * num_iiwa_positions, ki=[1] * num_iiwa_positions, kd=[20] * num_iiwa_positions, has_reference_acceleration=False)) iiwa_controller.set_name("iiwa_controller") builder.Connect(plant.get_state_output_port(iiwa), iiwa_controller.get_input_port_estimated_state()) # Add in the feed-forward torque adder = builder.AddSystem(Adder(2, num_iiwa_positions)) builder.Connect(iiwa_controller.get_output_port_control(), adder.get_input_port(0)) # Use a PassThrough to make the port optional (it will provide zero values # if not connected). torque_passthrough = builder.AddSystem(PassThrough([0] * num_iiwa_positions)) builder.Connect(torque_passthrough.get_output_port(), adder.get_input_port(1)) builder.ExportInput(torque_passthrough.get_input_port(), "iiwa_feedforward_torque") builder.Connect(adder.get_output_port(), plant.get_actuation_input_port(iiwa)) # Add discrete derivative to command velocities. desired_state_from_position = builder.AddSystem( StateInterpolatorWithDiscreteDerivative( num_iiwa_positions, time_step, suppress_initial_transient=True)) desired_state_from_position.set_name("desired_state_from_position") builder.Connect(desired_state_from_position.get_output_port(), iiwa_controller.get_input_port_desired_state()) builder.Connect(iiwa_position.get_output_port(), desired_state_from_position.get_input_port()) # Export commanded torques. builder.ExportOutput(adder.get_output_port(), "iiwa_torque_commanded") builder.ExportOutput(adder.get_output_port(), "iiwa_torque_measured") builder.ExportOutput(plant.get_generalized_contact_forces_output_port(iiwa), "iiwa_torque_external") # Wsg controller. wsg_controller = builder.AddSystem(SchunkWsgPositionController()) wsg_controller.set_name("wsg_controller") builder.Connect(wsg_controller.get_generalized_force_output_port(), plant.get_actuation_input_port(wsg)) builder.Connect(plant.get_state_output_port(wsg), wsg_controller.get_state_input_port()) builder.ExportInput(wsg_controller.get_desired_position_input_port(), "wsg_position") builder.ExportInput(wsg_controller.get_force_limit_input_port(), "wsg_force_limit") wsg_mbp_state_to_wsg_state = builder.AddSystem( MakeMultibodyStateToWsgStateSystem()) builder.Connect(plant.get_state_output_port(wsg), wsg_mbp_state_to_wsg_state.get_input_port()) builder.ExportOutput(wsg_mbp_state_to_wsg_state.get_output_port(), "wsg_state_measured") builder.ExportOutput(wsg_controller.get_grip_force_output_port(), "wsg_force_measured") # Cameras. AddRgbdSensors(builder, plant, scene_graph, model_instance_prefix=camera_prefix) # Export "cheat" ports. builder.ExportOutput(scene_graph.get_query_output_port(), "geometry_query") builder.ExportOutput(plant.get_contact_results_output_port(), "contact_results") builder.ExportOutput(plant.get_state_output_port(), "plant_continuous_state") builder.ExportOutput(plant.get_body_poses_output_port(), "body_poses") diagram = builder.Build() diagram.set_name("ManipulationStation") return diagram
ycb_object_sdfs = os.listdir(ycb_object_dir) ycb_object_sdfs = [os.path.join(ycb_object_dir, path) for path in ycb_object_sdfs] # Add random objects to the scene. parser = Parser(mbp, scene_graph) n_objects = np.random.randint(1, 12) obj_ids = [] for k in range(n_objects): obj_i = np.random.randint(len(ycb_object_sdfs)) parser.AddModelFromFile( file_name=ycb_object_sdfs[obj_i], model_name="obj_ycb_%03d" % k) obj_ids.append(k+2) mbp.Finalize() # Optional: set up a meshcat visualizer for the scene. #meshcat = builder.AddSystem( # MeshcatVisualizer(scene_graph, draw_period=0.0333)) #builder.Connect(scene_graph.get_pose_bundle_output_port(), # meshcat.get_input_port(0)) # Figure out where we're putting the camera for the scene by # pointing it inwards, and then applying a random yaw around # the origin. cam_quat_base = RollPitchYaw( 68.*np.pi/180., 0.*np.pi/180, 38.6*np.pi/180.).ToQuaternion() cam_trans_base = np.array([0.47, -0.54, 0.31])