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)