GolemGoalie Using Lqr Tree

## Project Abstract:

Making Golem Krang trace the randomly and continuously thrown ball and batting it involves delicate planning. We propose in this project an LQR-Tree based system using the current robot and ball state as its input variables. The goal is to find the optimal batting spot in space and time. This system will be implemented in srlib simulation we developed in Mike's Humanoids class.

## Related Work:

• R. Tedrake, "LQR-Trees: Feedback motion planning on sparse randomized trees" In Proceedings of Robotics: Science and Systems (RSS), page 8, 2009.
• This paper presents a feedback motion planning algorithm which combines locally-valid linear quadratic regulator (LQR) controllers into a nonlinear feedback policy which probabilistically covers the reachable area of a state space with a region of stability, certifying that all initial conditions that are capable of reaching the goal will stabilize to the goal.
• See the sample experiment from the author:Video.
• R. Burridge, A. Rizzi, D. Koditschek, "Sequential Composition of Dynamically Dexterous Robot Behaviors" The International Journal of Robotics Research, 1999
• This paper proposed a sequential controller composition technique for dexterous batting robots. A robot with a flat paddle is required to strike repeatedly at a thrown ball until the ball is safely brought to rest on the paddle at a specified location.
• S.H. Hyon, J. Moren, and G. Cheng. "Humanoid batting with bipedal balancing", 8th IEEE-RAS International Conference on Humanoid Robots, 2008. Humanoids 2008, 493–499, 2008.
• This paper integrated perception, control, and learning to allow a humanoid robot to do baseball batting. The ball was thrown by a human and recognized by the eye cameras. For the prediction, they proposed a simple sequential estimator to predict the arrival time and position of the ball. For the control, fast and smooth batting trajectories are superposed on a whole-body force controller taking account of bipedal balancing.
• Video .
• H. Watanabe, Y. Nihna, Y. Masutani, and F. Miyazaki. "Vision-based motion control for a hitting task-hanetsuki", Intelligent Robots and Systems' 94.'Advanced Robotic Systems and the Real World', IROS'94. Proceedings of the IEEE/RSJ/GI International Conference on, volume 2, 1994.
• The paper proposed a learning method with a novel database for the robot's Hanetsuki task (Japanese badminton).
• M. M. Williamson. "Robot Arm Control Exploiting Natural Dynamics", Massachusetts Institute of Technology, 1999.
• This Ph.D. thesis presented an approach to robot arm control exploiting natural dynamics. The approach consists of using an arm whose joints are controlled with simple non-linear oscillators. The resonant properties of the arm were exploited to perform a variety of rhythmic and discrete tasks such as juggling, sawing wood, throwing/hitting balls, hammering nails and drumming.

## Proposed Work:

Planning Challenges

The most interesting part in our project is a lot of dynamics involved in the problem and this raises the challenge as we design the planner.

First, we need to consider a dynamics system with two separate objects: the whole body motion of the robot and also the dynamics of the ball. For each coming ball, our planner must be able to solve the continuous system and find the path for the robot to reach the hitting position. Because we are dealing with the whole body, the configuration of hitting the ball could be very different. How to determine the best configuration is an important topic.

Meanwhile, the ball comes one after one, which means there is only limited time to achieve the goal state. The point is that it might be impossible to move from one configuration to some particular configuration in time, so we need to figure out what the optimal trajectory is in order to reach an acceptable state in time.

Proposed Solutions to Planning Challenges

We intend to apply the idea of LQR-tree to solve the continuous system. The timing is a big issue in our problem so one thing particularly good about LQR-tree is we can solve the optimal time by adding a term in the cost function.

Implementation Challenges

Due to the limited time we have for this final project, we will implement the whole idea in simulation first. The ultimate goal is to get all the things work on the humanoid robot Golem Krang, so we will use the existing simulation model of Golem Krang in srlib. (http://blip.tv/file/2033981/)

Dealing with the whole body control of Golem Krang would be a challenge because the movement of the arm and torso will affect the balance of the robot.

Proposed Solutions to Implementation Challenges

To simplify the implementation and planning issues, we can fix the torso for now, and look the body as one single link. We can also fix some DOF of the arm so that the movement of the arm will not deteriorate the balance of the robot.

## Timeline

Date General Task Kasemsit Hae Won Hai-Ning
Week 1 10/19-10/25 Write this document / Create a presentation for the class / Split work wiki/ppt wiki/ppt wiki/ppt
Week 2 10/26-11/1 Work on Project 2 & brainstorm proj2/bs proj2/bs proj2/bs
Week 3 11/2-11/8 Detailed brainstorming on LQR-tree / setup Simulation Environment / Model Ball dynamics LQR Ball Dynamics Simulation
Week 4 11/9-11/15 Implement LQR-Tree / Finalize Simulation Environment LQR tree for simple pendulum Ball dynamics in simulation LQR tree in simulation
Week 5 11/16-11/22 Implement & Test LQR-Tree LQR tree discussion LQR tree discussion LQR tree discussion
Week 6 11/23-11/29 Combine with Simulation LQR tree for two link Heading Control LQR tree in simulation
Week 7 11/30-12/6 Debug LQR tree testing Heading Control in simulation LQR tree in simulation
Week 8 12/7-12/11 Final Presentation / Report Report Report Report

## Week 1

<Be able to answer these questions>
How can LQR-tree be applied to trajectory planning?
How should we control the angle of the paddle?
How should we control the force of the paddle? (might be included in trajectory planning)

## Week 2

<Oct 19>
Topic changed from "juggling with an arm" to "batting with Golem Krang".
Hardware proposed during the meeting was,

Utilize Krang's segway with waist joint fixed to simplify the problem.
Fix Schunk arms' joints except the shoulder on one of the arms.

Prepare for presentation. Complete the Wiki. Kasemsit lost the rock-paper-scissors and is presenting, yes!

<Oct 20>
Blocking an arrow or a bullet with a shield idea was proposed during class.
Awesome idea, though wondering if LQR-Tree could be computed VERY fast.

## Week 3

<Oct 29>
Goal:

1. Clear definition for our problem
2. Get the Golem simulation working (Haining)
4. Dynamics of the robot and predicting the ball position

Problem formulation:

1. Perception of the ball state. Predict the ball trajectory. (Hae Won)
2. Decide the meeting point of the robot and the ball. (Hae Won)
3. Given the current point and the meeting point, determine the desired configuration of the robot at meeting point. (Kasemsit)
4. Generate a plan from current configuration to the goal configuration using LQR-tree. (Kasemsit)
• Easier task: move to the meeting point and then execute the plan to the desired configuration.
• Harder task: robot changes its configuration to the goal state while moving to the meeting point.

The point of this research:
How to use LQR-tree to generate a series of movements from current state to the goal state while moving to the goal position, and the plan must be finished before the robot reaches the meeting point.

## Week 4

<Nov 2>

• Derive basic dynamics for heading control
(1)
\begin{eqnarray} \Sigma\tau &=& I\alpha \\ FW &=& I\ddot{\theta} \\ \ddot{\theta} &=& \frac{W}{Ir_1}\tau \\ \end{eqnarray}
• Suppose we want to achieve $\theta = \theta_r$ and $\dot{\theta} = \dot{\theta}_r$. For a simple PD controller, let $\ddot{\theta}= -k_1(\theta - \theta_r) - k_2(\dot{\theta} - \dot{\theta}_r)$ for $k_1$, $k_2 > 0$. We have
(2)
\begin{align} \tau= -\frac{Ir_1}{W}[k_1(\theta - \theta_r) + k_2(\dot{\theta} - \dot{\theta}_r)] \end{align}
• The state-space representation can also obtained. Suppose we define $x_1 = \theta$, $x_2 = \dot{\theta}$, and $u = \tau$, then we have
(3)
\begin{eqnarray} \dot{x}_1 &=& x_2 \\ \dot{x}_2 &=& \frac{W}{Ir_1}u \\ \end{eqnarray}

<Nov 3>

• Dynamics equation of a two-wheeled one-linked robot where $I = I_0+(m_0+m_1)r^2$.
(4)
\begin{eqnarray} \tau_1 &=& (\ddot{q}_0+\ddot{q}_1)I+m_1 r \ell \ddot{q}_1 \cos{q_1}-m_1 r \ell \dot{q}^2_1 \sin{q_1} \\ 0 &=& (I+I_1+m_1\ell^2 +2 m_1 r \ell\cos{q_1})\ddot{q}_1 -g m_1 \ell \sin{q_1} + (I+ m_1 r \ell \cos{q_1})\ddot{q}_0 - m_1 r \ell\dot{q}^2_1 \sin{q_1} \\ \end{eqnarray}
• Ball dynamics on Simulation - Trajectory generation method (generate trajectory and then play in simulation)

## Week 5

• After discussing with Mike, we changed our robot model to full 2-linked model.
• Try LQR-Tree on simple pendulum in MATLAB.

## Week 6

• Generated many different trees for the 2-linked model in matlab.
• The first attempt to apply LQR-Tree on Krang in MATLAB.

• Simulation ready to execute LQR-Tree.
• Heading Control implemented in two different ways. PD control didn't work, solved by open-loop control

## Week 7

• Translating a single tree over the whole space has been discussed. Tested on srlib.

• Figured out why PD heading control was not working. Inertia on the wheels were set too big. Heading control provides smooth trajectory.

## Week 8

• Prepare the presentation. Write the report
• The presentation is done!! 95% of the paper is done!!!
• srlib simulation

page revision: 107, last edited: 11 Dec 2009 20:58