Getting Started

Note

If you haven’t already done so, make sure you’ve completed the steps in Basic before starting tutorial of the robotics part. The assets (robot) used in this this tutorial can be found here

Set up engine, renderer and scene

First setup the simulation engine, create a simulation scene and visualize it using renderer controller like before

import sapien.core as sapien


def robot_basic_control_demo(fix_robot_root, balance_passive_force, add_joint_damping):
    sim = sapien.Engine()
    renderer = sapien.OptifuserRenderer()
    renderer.enable_global_axes(False)
    sim.set_renderer(renderer)
    renderer_controller = sapien.OptifuserController(renderer)
    renderer_controller.set_camera_position(-2, 0, 1)
    renderer_controller.set_camera_rotation(0, -0.5)

    scene0 = sim.create_scene(gravity=[0, 0, -9.81])
    renderer_controller.set_current_scene(scene0)
    scene0.add_ground(altitude=0)
    scene0.set_timestep(1 / 240)
    scene0.set_ambient_light([0.5, 0.5, 0.5])
    scene0.set_shadow_light([0, 1, -1], [0.5, 0.5, 0.5])

Load robot and set joint position

Then we create a URDF loader to load the Kinova Jaco2 arm from URDF file and set the robot to the initial position. Note that this is a fix_root_link flag for URDF loader. If it is set to true, then the robot root will be fixed to world coordinates, otherwise the root link will be free with all 6 degree of freedom.

loader = scene0.create_urdf_loader()
loader.fix_root_link = fix_robot_root
robot = loader.load("assets/robot/jaco2.urdf")
robot.set_root_pose(sapien.Pose([0, 0, 0], [1, 0, 0, 0]))

arm_init_qpos = [4.71, 2.84, 0, 0.75, 4.62, 4.48, 4.88]
gripper_init_qpos = [0, 0, 0, 0, 0, 0]
init_qpos = arm_init_qpos + gripper_init_qpos
robot.set_qpos(init_qpos)

Like before, run simulation step and on-screen rendering.

    steps = 0
    renderer_controller.show_window()
    while not renderer_controller.should_quit:
        scene0.update_render()
        for i in range(4):
            if balance_passive_force:
                qf = robot.compute_passive_force(gravity=True, coriolis_and_centrifugal=True, external=False)
                robot.set_qf(qf)
            scene0.step()
            steps += 1
        renderer_controller.render()

Change flag of URDF Loader

Now we run all the code with robot_basic_control_demo(False, False, False)

It is expected that you will observe the following “falling-down” robot arm. This is caused by the fix_root_link flag, which is set to False before load the robot.

Change fix_robot_root to True and rerun the code, we still find that the robot arm can not get to the initial position as we set. Actually, at the first simulation step, the robot is set to our desired joint angle. However, due to gravitational force and other possible passive force like Coriolis and Centrifugal force, the Jaco2 arm will immediately drop down.

Note

When a robot is already loaded, change flags of URDF loader will have no effect on it.

Gravity compensation

For a real robot, the gravity compensation is done by a internal controller hardware. So it is usually desirable to skip this troublesome calculation of how to compensate gravity.

In SAPIEN, we use compute_passive_force to do that. It calculates the joint force/torque in order to compensate specified passive force. Here we only consider gravity, coriolis and centrifugal force.

    qf = robot.compute_passive_force(gravity=True, coriolis_and_centrifugal=True, external=False)
    robot.set_qf(qf)

Now we run all the code with robot_basic_control_demo(True, True, False)

We can see the robot can stay at the target pose for a short period as following. However, it will then deviate from this pose gradually due to numerical error.

Add damping to joints

To make the robot stay at this pose, we need to add some damping to the robot joint. It will penalize the joint velocity to stabilize the robot. Add the following code before the while loop.

if add_joint_damping:
    for joint in robot.get_joints():
        joint.set_drive_property(stiffness=0, damping=10)

Now we run all the code with robot_basic_control_demo(True, True, True)

The entire code

After that, our robot can keep it pose at the given joint angle. The entire code is as follow:

import sapien.core as sapien


def robot_basic_control_demo(fix_robot_root, balance_passive_force, add_joint_damping):
    sim = sapien.Engine()
    renderer = sapien.OptifuserRenderer()
    renderer.enable_global_axes(False)
    sim.set_renderer(renderer)
    renderer_controller = sapien.OptifuserController(renderer)
    renderer_controller.set_camera_position(-2, 0, 1)
    renderer_controller.set_camera_rotation(0, -0.5)

    scene0 = sim.create_scene(gravity=[0, 0, -9.81])
    renderer_controller.set_current_scene(scene0)
    scene0.add_ground(altitude=0)
    scene0.set_timestep(1 / 240)
    scene0.set_ambient_light([0.5, 0.5, 0.5])
    scene0.set_shadow_light([0, 1, -1], [0.5, 0.5, 0.5])

    loader = scene0.create_urdf_loader()
    loader.fix_root_link = fix_robot_root
    robot = loader.load("assets/robot/jaco2.urdf")
    robot.set_root_pose(sapien.Pose([0, 0, 0], [1, 0, 0, 0]))

    arm_init_qpos = [4.71, 2.84, 0, 0.75, 4.62, 4.48, 4.88]
    gripper_init_qpos = [0, 0, 0, 0, 0, 0]
    init_qpos = arm_init_qpos + gripper_init_qpos
    robot.set_qpos(init_qpos)

    if add_joint_damping:
        for joint in robot.get_joints():
            joint.set_drive_property(stiffness=0, damping=10)

    steps = 0
    renderer_controller.show_window()
    while not renderer_controller.should_quit:
        scene0.update_render()
        for i in range(4):
            if balance_passive_force:
                qf = robot.compute_passive_force(gravity=True, coriolis_and_centrifugal=True, external=False)
                robot.set_qf(qf)
            scene0.step()
            steps += 1
        renderer_controller.render()

    scene0 = None


if __name__ == '__main__':
    robot_basic_control_demo(fix_robot_root=True, balance_passive_force=True, add_joint_damping=True)