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

To make it short and sweet: policy iteration doesn't work, especially not for biped walking. The reason is that in your model I've counted at least 8 joints and moving them to the right position is some kind of mammoth project. But it don't want to be pessimistic. Let us first describe, what is ok in the project. Selecting the python programming language is a good choice, because Python works for prototyping. It's possible to change the sourcecode without much effort. What is also reasonable well is the pybullet library, which a physics engine for creating realistic animations and the OpenAI gym is also a good choice as an integrated development system.

Now, let us go to the things which are not working. The idea of determine the joints of the biped walker only by reinforcement learning is some kind of anti-pattern. Perhaps a small sidestep: reinforcement learning is a fluctuating algorithm which is testing out different policies at random and improves thems into the direction of the smallest error. It works similar to genetic programming. This concept works only, if the state space is really small, which isn't the case in the example of walking trajectories of a robot.

My advice is to use natural language for storing domain specific knowledge. A walking gait contains of phases like init, swing, up, down, contact, swing. These phases are called a walk-cycle and are used in computer animation for manual scripting characters. It's important to integrate this knowledge in the system to make the state space smaller. On top of the expert knowledge minor modifications can be trained with deep learning.

Literature The concept of “natural language” for injecting domain specific knowledge into the motion controller is described under the term “Fuzzy control with linguistic variables” in the literature:

• Liu, Zhi, Yun Zhang, and Yaonan Wang. "A type-2 fuzzy switching control system for biped robots." IEEE Transactions on Systems, Man, and Cybernetics, Part C (Applications and Reviews) 37.6 (2007): 1202-1213.
• Katić, Duško, and Miomir Vukobratović. "Control algorithm for humanoid walking based on fuzzy reinforcement learning." (2006).

Grammatical control is trying to adapt this principle in a more elaborated way for biped walking in robotics.

• quote: “The step length as a walking parameter is defined by compiling the text file of Grammar Control.” Lim, Hun-ok, Yoshiharu Kaneshima, and Atsuo Takanishi. "Online walking pattern generation for biped humanoid robot with trunk." Robotics and Automation, 2002. Proceedings. ICRA'02. IEEE International Conference on. Vol. 3. IEEE, 2002.

Answer 2

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 3

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.