def _setup(self, shape): # Load the right URDF, and create robot. if shape == ShapeType.POINT: urdf_name = "point_mass.urdf" elif shape == ShapeType.BOX: urdf_name = "box_collision_mesh.urdf" elif shape == ShapeType.SPHERE: urdf_name = "sphere_primitive.urdf" # Create the jiminy robot and controller robot = load_urdf_default(urdf_name, has_freeflyer=True) # Add contact point or collision body, along with related sensor, # depending on shape type. if shape != ShapeType.POINT: # Add collision body robot.add_collision_bodies([self.body_name]) # Add a force sensor force_sensor = jiminy.ForceSensor(self.body_name) robot.attach_sensor(force_sensor) force_sensor.initialize(self.body_name) else: # Add contact point robot.add_contact_points([self.body_name]) # Add a contact sensor contact_sensor = jiminy.ContactSensor(self.body_name) robot.attach_sensor(contact_sensor) contact_sensor.initialize(self.body_name) # Extract some information about the engine and the robot m = robot.pinocchio_model.inertias[-1].mass g = robot.pinocchio_model.gravity.linear[2] weight = m * np.linalg.norm(g) if shape == ShapeType.POINT: height = 0.0 elif shape == ShapeType.BOX: geom = robot.collision_model.geometryObjects[0] height = geom.geometry.points(0)[2] # It is a mesh, not an actual box primitive elif shape == ShapeType.SPHERE: geom = robot.collision_model.geometryObjects[0] height = geom.geometry.radius frame_idx = robot.pinocchio_model.getFrameId(self.body_name) joint_idx = robot.pinocchio_model.frames[frame_idx].parent frame_pose = robot.pinocchio_model.frames[frame_idx].placement return robot, weight, height, joint_idx, frame_pose
def setUp(self): # Load URDF, create robot. urdf_path = "data/point_mass.urdf" # Define the parameters of the contact dynamics self.k_contact = 1.0e6 self.nu_contact = 2.0e3 self.v_stiction = 5e-2 self.r_stiction = 0.5 self.dry_friction = 5.5 self.visc_friction = 2.0 self.dtMax = 1.0e-5 # Create the jiminy robot and controller self.robot = jiminy.Robot() self.robot.initialize(urdf_path, has_freeflyer=True) self.robot.add_contact_points(['MassBody']) force_sensor = jiminy.ForceSensor('MassBody') self.robot.attach_sensor(force_sensor) force_sensor.initialize('MassBody')
motor_joint_names = ("PendulumJoint", ) force_sensor_def = { "F1": "Corner1", "F2": "Corner2", "F3": "Corner3", "F4": "Corner4" } robot = jiminy.Robot() robot.initialize(urdf_path, True) for joint_name in motor_joint_names: motor = jiminy.SimpleMotor(joint_name) robot.attach_motor(motor) motor.initialize(joint_name) for sensor_name, frame_name in force_sensor_def.items(): force_sensor = jiminy.ForceSensor(sensor_name) robot.attach_sensor(force_sensor) force_sensor.initialize(frame_name) robot.add_contact_points(contact_points) # Extract some constant iPos = robot.motors_position_idx[0] iVel = robot.motors_velocity_idx[0] axisCom = 1 # Constants m = 75 l = 1.0 g = 9.81 omega = np.sqrt(g / l)