Skip to content

Manipulator Envs

cbfpy.envs.arm_envs

Simulation environents for robot arms

This currently includes a very simple 3-DOF environment which helps demonstrate joint limit avoidance, but more will be added in the future

JointLimitsEnv

Bases: BaseEnv

Simulation environment for the 3-DOF arm joint-limit-avoidance demo

This includes a desired reference trajectory which is unsafe: it will command sinusoidal joint motions (with different frequencies per link) that will exceed the joint limits of the robot

Source code in cbfpy/envs/arm_envs.py 
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
class JointLimitsEnv(BaseEnv):
    """Simulation environment for the 3-DOF arm joint-limit-avoidance demo

    This includes a desired reference trajectory which is unsafe: it will command sinusoidal
    joint motions (with different frequencies per link) that will exceed the joint limits of the robot
    """

    def __init__(self):
        with stdout_redirected(restore=False):
            self.client: pybullet = BulletClient(pybullet.GUI)
        self.robot = pybullet.loadURDF(URDF, useFixedBase=True)
        self.num_joints = self.client.getNumJoints(self.robot)
        self.q_min = np.array(
            [self.client.getJointInfo(self.robot, i)[8] for i in range(self.num_joints)]
        )
        self.q_max = np.array(
            [self.client.getJointInfo(self.robot, i)[9] for i in range(self.num_joints)]
        )
        self.timestep = self.client.getPhysicsEngineParameters()["fixedTimeStep"]
        self.t = 0

        # Sinusoids for the desired joint positions
        # Setting the amplitude to be the full joint range means we will command DOUBLE
        # the joint range, exceeding our limits
        self.omegas = 0.1 * np.array([1.0, 2.0, 3.0])
        self.amps = self.q_max - self.q_min
        self.offsets = np.zeros(3)

    def step(self):
        self.client.stepSimulation()
        self.t += self.timestep

    def get_state(self):
        states = self.client.getJointStates(self.robot, range(self.num_joints))
        return np.array([states[i][0] for i in range(self.num_joints)])

    def get_desired_state(self):
        # Evaluate our unsafe sinusoidal trajectory
        return self.amps * np.sin(self.omegas * self.t) + self.offsets

    def apply_control(self, u):
        self.client.setJointMotorControlArray(
            self.robot,
            list(range(self.num_joints)),
            self.client.VELOCITY_CONTROL,
            targetVelocities=u,
        )

_test_env()

Test the environment behavior under an unsafe nominal controller

Source code in cbfpy/envs/arm_envs.py 
67
68
69
70
71
72
73
74
75
76
77
78
79
80
def _test_env():
    """Test the environment behavior under an **unsafe** nominal controller"""

    def nominal_controller(z, z_des):
        k = 1.0
        return k * (z_des - z)

    env = JointLimitsEnv()
    while True:
        z = env.get_state()
        z_des = env.get_desired_state()
        u = nominal_controller(z, z_des)
        env.apply_control(u)
        env.step()