How to implement a Continuous Control of a quadruped robot with Deep Reinforcement Learning in Pybullet and OpenAI Gym?

Question

Description

I have designed this robot in URDF format and its environment in pybullet. Each leg has a minimum and maximum value of movement.

What reinforcement algorithm will be best to create a walking policy in a simple environment in which a positive reward will be given if it walks in the positive X-axis direction?

I am working in the following but I don´t know if it is the best way:

The expected output from the policy is an array in the range of (-1, 1) for each joint. The input of the policy is the position of each joint from the past X frames in the environment(replay memory like DeepQ Net), the center of mass of the body, the difference in height between the floor and the body to see if it has fallen and the movement in the x-axis.

Limitations

left_front_joint => lower="-0.4" upper="2.5" id=0

left_front_leg_joint => lower="-0.6" upper="0.7" id=2

right_front_joint => lower="-2.5" upper="0.4" id=3

right_front_leg_joint => lower="-0.6" upper="0.7" id=5

left_back_joint => lower="-2.5" upper="0.4" id=6

left_back_leg_joint => lower="-0.6" upper="0.7" id=8

right_back_joint => lower="-0.4" upper="2.5" id=9

right_back_leg_joint => lower="-0.6" upper="0.7" id=11

The code below is just a test of the environment with a set of movements hardcoded in the robot just to test how it could walk later. The environment is set to real time, but I assume it needs to be in a frame by frame lapse during the policy training. (p.setRealTimeSimulation(1) #disable and p.stepSimulation() #enable)

A video of it can be seen in:

https://youtu.be/j9sysG-EIkQ

The complete code can be seen here:

https://github.com/rubencg195/WalkingSpider_OpenAI_PyBullet_ROS

CODE

import pybullet as p
import time
import pybullet_data

def moveLeg( robot=None, id=0, position=0, force=1.5  ):
    if(robot is None):
        return;
    p.setJointMotorControl2(
        robot,
        id,
        p.POSITION_CONTROL,
        targetPosition=position,
        force=force,
        #maxVelocity=5
    )

pixelWidth = 1000
pixelHeight = 1000
camTargetPos = [0,0,0]
camDistance = 0.5
pitch = -10.0
roll=0
upAxisIndex = 2
yaw = 0

physicsClient = p.connect(p.GUI)#or p.DIRECT for non-graphical version
p.setAdditionalSearchPath(pybullet_data.getDataPath()) #optionally
p.setGravity(0,0,-10)
viewMatrix = p.computeViewMatrixFromYawPitchRoll(camTargetPos, camDistance, yaw, pitch, roll, upAxisIndex)
planeId = p.loadURDF("plane.urdf")
cubeStartPos = [0,0,0.05]
cubeStartOrientation = p.getQuaternionFromEuler([0,0,0])
#boxId = p.loadURDF("r2d2.urdf",cubeStartPos, cubeStartOrientation)
boxId = p.loadURDF("src/spider.xml",cubeStartPos, cubeStartOrientation)
# boxId = p.loadURDF("spider_simple.urdf",cubeStartPos, cubeStartOrientation)



toggle = 1



p.setRealTimeSimulation(1)

for i in range (10000):
    #p.stepSimulation()


    moveLeg( robot=boxId, id=0,  position= toggle * -2 ) #LEFT_FRONT
    moveLeg( robot=boxId, id=2,  position= toggle * -2 ) #LEFT_FRONT

    moveLeg( robot=boxId, id=3,  position= toggle * -2 ) #RIGHT_FRONT
    moveLeg( robot=boxId, id=5,  position= toggle *  2 ) #RIGHT_FRONT

    moveLeg( robot=boxId, id=6,  position= toggle *  2 ) #LEFT_BACK
    moveLeg( robot=boxId, id=8,  position= toggle * -2 ) #LEFT_BACK

    moveLeg( robot=boxId, id=9,  position= toggle *  2 ) #RIGHT_BACK
    moveLeg( robot=boxId, id=11, position= toggle *  2 ) #RIGHT_BACK
    #time.sleep(1./140.)g
    #time.sleep(0.01)
    time.sleep(1)

    toggle = toggle * -1

    #viewMatrix        = p.computeViewMatrixFromYawPitchRoll(camTargetPos, camDistance, yaw, pitch, roll, upAxisIndex)
    #projectionMatrix  = [1.0825318098068237, 0.0, 0.0, 0.0, 0.0, 1.732050895690918, 0.0, 0.0, 0.0, 0.0, -1.0002000331878662, -1.0, 0.0, 0.0, -0.020002000033855438, 0.0]
    #img_arr = p.getCameraImage(pixelWidth, pixelHeight, viewMatrix=viewMatrix, projectionMatrix=projectionMatrix, shadow=1,lightDirection=[1,1,1])

cubePos, cubeOrn = p.getBasePositionAndOrientation(boxId)
print(cubePos,cubeOrn)
p.disconnect()

Answer 1

http://www-anw.cs.umass.edu/~barto/courses/cs687/Kohl-Stone-04.pdf
Here's a paper about quadrupedal Aibo robots learning how to walk. They first hand tuned parameters for walking and then used policy gradient method to 'polish' the parameters. Maybe you can get some ideas from their work.

Answer 2

Proposed Approach

The expected output from the policy is an array in the range of (-1, 1) for each joint. The input of the policy is the position of each joint from the past X frames in the environment (replay memory like DeepQ Net), the center of mass of the body, the difference in height between the floor and the body to see if it has fallen and the movement in the x-axis.

The code below is just a test of the environment with a set of movements hard coded in the robot just to test how it could walk later. The environment is set to real time, but I assume it needs to be in a frame by frame lapse during the policy training.

Initial Considerations

It is advisable to first list and analyze the use cases, system requirements, and their design consequences. The following details can be addressed after a high level theory and design have been established.

• Algorithm details

• Models according to schemas such as URDF

• Physics simulators, such as Bullet

• Challenge frameworks, such as Gym

• Goals in terms of Cartesian direction of travel

• Number of limbs

• Program variable names

• Initialization

Some requirements are generally assumed as part of any useful robotic controller and need not be mentioned explicitly.

• Continuous control (discontinuous control and lack of control are poor options)

• Hinged movement always has a range (min and max)

Listing more relevant system design constraints such as these can guide analysis and design. This is an abridged list to consider as an example.

• No precision in the control of angular speed and velocity at joints.

• No vision, audio, radar, sonar, or lidar sensory information available for either position sensing or predictive path planning

• Orientation is not directly sensed

• Surface coordinates are sensed via triangulation, and that subsystem is already designed and tested

• Clearance to ground is known and must be maintained above a configurable constant value

• The use of Deep Reinforcement Learning is expected (which, given the mechanical design, implies the maintenance of a walking policy)

• The goal is to maintain a particular direction of robot travel

• Each limb has two radial degrees of freedom, controlled by an angular position command input to the motion control sub-system

• The nominal radial acceleration and maximum radial velocity of angular positions are not known

Extensions of Traditional Control

The AI system must produce stability. An established method is closed loop control. The stability performance of the AI system must at least exceed standard PID control.

$$ k_p \epsilon(t) + k_i \int_0^t \epsilon(t) dt + k_d \frac {d\epsilon(t)} {dt} $$

The integral term can have an inverse exponential memory decay as such.

$$ k_p \epsilon(t) + k_i \frac {\int_0^t e^t \epsilon(t) dt} {\int_0^t e^t dt} + k_d \frac {d\epsilon(t)} {dt} $$

Nonetheless, all terms are first degree in $k$ parameters. With a feed forward network, some competition winning robotics systems have reached a higher level of performance in terms of stability and efficiency of motion.

$$ f_p(P) \epsilon(t) + f_i(P) \frac {\int_0^t e^t \epsilon(t) dt} {\int_0^t e^t dt} + f_d(P) \frac {d\epsilon(t)} {dt} $$

This is not yet tractable, since offline learning will not take advantage of terrain knowledge gained during a robot's walk. If the terrain is a constant, for instance, if it is always a flat plane, and the positioning system is stable and reliable, reinforcement would not be needed. Such an ideal environment for the walk is rare in practice.

Most servo controllers with radial encoder inputs provide positional and dynamic stability using a model similar to the second of the above three equations.

Deep Reinforcement Learning

The development of reinforced learning policy updating is desirable for real world walker control. Although a policy update function can theoretically be learned from walk examples, the distribution of terrain is not easily representable in example terrain, even if the terrains are generated by genetic-algorithm-like designs modified to simulate planetary geological processes over cosmic time frames.

The approaches that show early indications of success involve reward $r$, dynamics $p$, and policy $\pi$ that depend on both on state and action.

$$ \pi (u_t | x_t) \\ p(x_t | x_t, u_t) \\ r(x_t, u_t) $$

The study of variants of NAF that leverage replay memory is outlined in Deep Reinforcement Learning for Robotic Manipulation with Asynchronous Off-Policy Updates, Shixiang Gu, Ethan Holly, Timothy Lillicrap, Sergey Levine, 2016, using this model.

$$ \mu_{n+1} (x_t) := {argmax}_u{Q_{{\pi}_n} (x_t, u_t)} \\ f() := \tanh() \; \text{,} \\ \mu(x) = f(k + K_x) \\ Q(x,u) = \frac 1 2 {\big(u − \mu(x)\big)}^T P(u − \mu(x)) + x^T (B x + b) + c $$

where P, k, K, B, b, and c are learnable tensors. Gu et. al. explains.

If f() [was instead] identity, then the expression corresponds to a globally quadratic Q-function and a linear feedback policy, though due to the Tanh non-linearity, the Q-function is not linear with respect to state-action features.

Reward

Because of the above considerations, defining reward solely on the basis of one axis of position will likely be insufficient to guide effective learning. Various actions have costs in terms of both time and energy consumption and each may affect future move choices in significant ways. Violation of the ground clearance constraint must produce a penalty. The reward function must guide policy development to properly take these factors into consideration.

To enhance the tractability of the learning challenge, a process block might best be inserted between the deep reinforcement learning component and the controller to perform a trigonometric and Newtonian translation from desired foot positions relative to the center of robot mass and the radial positions on all control axes to achieve those positions. That removes the burden of that translation from the walker's learner.