Create Articulations
The articulation is a set of links (each of which behaves like a rigid body) connected together with special joints. For instance, a drawer can be connected to a table by a prismatic joint (slider), and a door can be connected to a frame by a revolute joint (hinge). A robot is also an instance of an articulation. Articulations are usually loaded from URDF XML, which we will see in other examples. This tutorial showcases how to create an articulation programmatically by SAPIEN.
Articulations can be controlled by applying torques on their joints. To drive an articulation to a desired state, users can apply a controller to compute corrective torques according to the difference between the actual and the desired.
In this tutorial, you will learn the following:
Create
Articulation
Control the articulation with builtin controllers
Get kinematic quantities of the articulation
The example illustrates how to build a controllable toy car from scratch.
transforms3d
is required.
The full script can be downloaded here create_articulations.py
Create a root link
In SAPIEN, the articulation is represented as a tree. Each node is a link, and each edge indicates that the child link is connected to the parent link by a joint. To build a toy car, let’s start with the car body.
def create_car(
scene: sapien.Scene,
body_size=(1.0, 0.5, 0.25),
tire_radius=0.15,
joint_friction=0.0,
joint_damping=0.0,
density=1.0,
) -> sapien.Articulation:
body_half_size = np.array(body_size) / 2
shaft_half_size = np.array([tire_radius * 0.1, tire_radius * 0.1, body_size[2] * 0.1])
rack_half_size = np.array([tire_radius * 0.1, body_half_size[1] * 2.0, tire_radius * 0.1])
builder: sapien.ArticulationBuilder = scene.create_articulation_builder()
# car body (root of the articulation)
body: sapien.LinkBuilder = builder.create_link_builder() # LinkBuilder is similar to ActorBuilder
body.set_name('body')
body.add_box_collision(half_size=body_half_size, density=density)
body.add_box_visual(half_size=body_half_size, color=[0.8, 0.6, 0.4])
Articulation
is created by ArticulationBuilder
.
Each link is built by LinkBuilder
, which can be created by an articulation builder.
A link is just a rigid body, and thus collision and visual shapes can be added.
A root link is created when create_link_builder()
is called without specifying the parent link.
Create a child link connected by a revolute joint
Next, we create a child link (front steering shaft) connected to the root link (car body) by a revolute joint.
# front steering shaft
front_shaft = builder.create_link_builder(body)
front_shaft.set_name('front_shaft')
front_shaft.set_joint_name('front_shaft_joint')
front_shaft.add_box_collision(half_size=shaft_half_size, density=density)
front_shaft.add_box_visual(half_size=shaft_half_size, color=[0.6, 0.4, 0.8])
# The x-axis of the joint frame is the rotation axis of a revolute joint.
front_shaft.set_joint_properties(
'revolute',
limits=[[-np.deg2rad(15), np.deg2rad(15)]], # joint limits (for each DoF)
# pose_in_parent refers to the relative transformation from the parent frame to the joint frame
pose_in_parent=sapien.Pose(
p=[(body_half_size[0] - tire_radius), 0, -body_half_size[2]],
q=euler2quat(0, -np.deg2rad(90), 0)
),
# pose_in_child refers to the relative transformation from the child frame to the joint frame
pose_in_child=sapien.Pose(
p=[0.0, 0.0, shaft_half_size[2]],
q=euler2quat(0, -np.deg2rad(90), 0)
),
friction=joint_friction,
damping=joint_damping,
)
A child link is created when create_link_builder(parent_link)
is called with specifying the parent link.
Besides, we need to configure the joint.
There are multiple types of joints: prismatic, revolute, fixed. The definitions follow PhysX.
revolute: a revolute joint (also called a hinge) keeps the origins and x-axes of the frames together, and allows free rotation around this common axis.
prismatic: a prismatic joint (also called a slider) keeps the orientations identical, but allows the origin of each frame to slide freely along the common x-axis.
fixed: a fixed joint locks the orientations and origins rigidly together
The location of the joint is defined by the joint pose in the parent frame pose_in_parent
, and the joint pose in the child frame pose_in_child
.
Other properties of a joint, like joint friction and joint damping, can also be set through set_joint_properties(...)
.
Control an articulation with builtin drives
After building the articulation, we want to control it by actuating its joints. SAPIEN provides builtin drives (controllers) to control either the position or the speed of a joint.
def get_joints_dict(articulation: sapien.Articulation):
joints = articulation.get_joints()
joint_names = [joint.name for joint in joints]
assert len(joint_names) == len(set(joint_names)), 'Joint names are assumed to be unique.'
return {joint.name: joint for joint in joints}
All the joints of an articulation can be acquired by get_joints()
.
Note
Although the order of joints returned by get_joints()
is fixed, it is recommended to index a joint by its name.
Joint names should be unique, which is not forced in SAPIEN though.
joints = get_joints_dict(car)
print(joints.keys())
joints['front_shaft_joint'].set_drive_property(stiffness=1000.0, damping=0.0) # front shaft
joints['front_gear'].set_drive_property(0.0, 1000.0) # front gear
joints['back_gear'].set_drive_property(0.0, 0.0) # back gear
For each active joint (with non-zero degree of freedom), we can set its drive properties: stiffness
and damping
.
They implies the extent to which the drive attempts to achieve the target position and velocity respectively.
There do not exist a general rule to set those values and you usually should tune them case by case.
If you are familiar with control theory, they correspond to P and D terms in PID controller.
The initial target position and velocity of a joint are zero by default.
Since our toy car is designed to be a front-wheel drive car, we set both the stiffness and damping as zero for the back gear.
Note
When a non-zero target is set and stiffness/damping is also non-zero, the drive takes effect internally at each simulation step.
We can implement different behaviors when different keys are pressed.
set_drive_target(...)
and set_drive_velocity_target(...)
are called to set the target position and velocity of a joint drive.
position = 0.0 # position target of joints
velocity = 0.0 # velocity target of joints
steps = 0
last_step = -4
while not viewer.closed:
if steps - last_step < 4:
pass # prevent multiple changes for one key press
else:
last_step = steps
if viewer.window.key_down('i'): # accelerate
velocity += 5.0
print('velocity increases:', velocity)
joints['front_gear'].set_drive_velocity_target(velocity)
elif viewer.window.key_down('k'): # brake
velocity -= 5.0
print('velocity decreases:', velocity)
joints['front_gear'].set_drive_velocity_target(velocity)
elif viewer.window.key_down('j'): # left turn
position += 2
position = np.clip(position, *limits)
joints['front_shaft_joint'].set_drive_target(np.deg2rad(position))
print('position increases:', position)
elif viewer.window.key_down('l'): # right turn
position -= 2
position = np.clip(position, *limits)
print('position decreases:', position)
joints['front_shaft_joint'].set_drive_target(np.deg2rad(position))
elif viewer.window.key_down('r'): # reset
position = 0
velocity = 0.0
print('reset')
joints['front_shaft_joint'].set_drive_target(position)
joints['front_gear'].set_drive_velocity_target(velocity)
Get kinematic quantities of the articulation
The pose of the articulation (frame) in the world frame can be acquired by get_pose()
.
It is the same as the pose of the root link.
Besides, joint positions and velocities can be acquired by get_qpos()
and get_qvel()
.
They both return a list of scalars, the length of which is the total degree of freedom.
The order is the same as get_joints()
.
print('Pose', car.get_pose())
print('Joint positions', car.get_qpos())
print('Joint velocities', car.get_qvel())
Remove an articulation
Similar to removing an actor, scene.remove_articulation(articulation)
will
remove it from the scene. Using the articulation or any of its links or joints
after removal will result in undefined behavior (usually a crash).