publish compliance extension to icub simulator · could also be used for different robots including...

EU FP7

AMARSi

Adaptive Modular Architectures for Rich Motor Skills

ICT-248311

D 2.2

Marh 2011 (1 year)

Publish compliance extension to iCub simulatorAuthors: Gustavo Medrano Cerda (IIT), Jody Saglia (IIT), Zhibin Li (IIT),

Dongming Gan (IIT), Nikos G. Tsagarakis (IIT),

Due date of deliverable 15st March 2011Actual submission date 6th April 2011Lead Partner IITRevision FinalDissemination level Public

This deliverable reports on the ongoing activities in Workpackage 2, and in particular thework within T2.2 related to the modeling of compliance of the currently under developmentcompliant humanoid robot. Modelling compliance in the robot drive system is critical sincecompliance in the drives has an impact on the design of control systems and the system perfor-manca and behavior in general. The specific goal of T2.2 is to develop open simulation toolsfor the Compliant Humanoid which can be used to assist the research and comparative studieswithin the AMARSi Project.

Contents

1. Introduction 2

2. Robotran-Matlab Simulator for Control of Compliant Humanoid 42.1. Existing Simulators for Humanoid Robots . . . . . . . . . . . . . . . . . . . 42.2. Robot and Actuator Dynamics . . . . . . . . . . . . . . . . . . . . . . . . . 62.3. Actuator Models of the Compliant Humanoid Platform . . . . . . . . . . . . 82.4. Waist Differential Drive Model with Yaw Rotation . . . . . . . . . . . . . . 142.5. ROBOTRAN: Symbolic Multibody Modelling . . . . . . . . . . . . . . . . . 202.6. A ROBOTRAN-MATLAB Simulator for the Compliant Humanoid . . . . . . 20

2.6.1. System Modelling . . . . . . . . . . . . . . . . . . . . . . . . . . . 222.6.2. Ground Impacts and Exchange of Support Leg . . . . . . . . . . . . 242.6.3. Robotran Model Transformations . . . . . . . . . . . . . . . . . . . 24

2.7. ROBOTRAN-MATLAB Simulator Demo Models . . . . . . . . . . . . . . . 25

3. AMARSi Humanoid Model for the OpenHRP Simulator 273.1. Using the OpenHRP Simulator for verifying locomotion trajectories for the

lower body . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 273.2. Simulations using OpenHRP and Experiments in the Prototype Lower body . 28

3.2.1. ZMP Trajectory Planning for Quasi-Dancing and Walking . . . . . . 283.2.2. Simulation Experiments of Dancing and Walking . . . . . . . . . . . 29

4. Impact Simulation and Stiffness Tuning for a 4-DOF Compliant Arm/LegRobotic System 344.1. System Dynamics and Worst Impact Configuration for Joint 1 of the 4-DOF

Arm/Leg robot . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 344.2. Impact Simulation and Stiffness Analysis for Joint 1 of the 4-DOF arm/leg robot 384.3. Impact Simulation Analysis of the Other Three Joints in the 4-DOF Arm/Leg

robot . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 42

A. Differential Drive Models in Matrix Form 46

References 49

1. Introduction

The development of realistic mathematical models and simulators is a time consuming andlaborious process. However, it is a vital stage in the analysis, design and manufacture ofcomplex systems. Mathematical models are also invaluable tools for the design of controlalgorithms and in this case, it is essential to include models for actuators and sensors. Rapidtesting can be carried out in the simulator before implementation in the real hardware, thusavoiding costly damages to equipment. Ideally, the model and simulator should be a faithfulrepresentation of the real system so that there is little or no difference between running asimulation and testing on the hardware.

Figure 1.1.: Compliant Humanoid legs: Compliant joints in ankles and knees.

The focus of this deliverable is on the modelling and simulation of humanoid and bipedalrobots. As such, models for rigid multibody systems need to be developed together with therelevant actuators and sensors (electric, hydraulic, pneumatic motors, gearboxes, belt/cabledrives, compliance, non-linear friction, sensor dynamics, noise/quantization). Models thatinclude compliance are essential for humanoids that have been built and designed using com-pliant elements, e.g. Flame and Compliant Humanoid, [10] Fig.1.1. Modelling compliance inthe robot drive system is important since high compliance in the drives will have an impacton the design of control systems. The specific goal of T2.2 is to develop an open simulator

2

1. Introduction

for Compliant Humanoid. This will help to foster research and comparative studies within theAMARSi Project.

This report is organized as follows. Chapter 2 presents a brief overview of some modelling andsimulation software packages and their shortcomings. Further, the chapter gives a descriptionof Robotran [7], a powerful tool for multibody system modelling that has been used to modelthe Compliant Humanoid. This work has been published in [14].

Chapter 3 presents the modeling of the Compliant Humanoid with the open source simu-lation environment OpenHRP. The chapter presents simulation and experimental results forquasi-dancing and walking tasks performed using the lower body model implemented in theOpenHRP simulator. The OpenHRP simulator of the humaonid lower body was used in thework published in [12, 13, 15].

Chapter 4 describes the impact simulation and joint stiffness tuning for a generic 4-DOFrobotic system which can be consider both as a robotic arm or leg. This simulation, developedin ADAMS, aims at defining the stiffness values for the compliant joints of the humanoidrobot. This work has been published in [11].

3

2. Robotran-Matlab Simulator forControl of Compliant Humanoid

2.1. Existing Simulators for Humanoid Robots

During the last twenty years numerous software packages for modelling multibody systemshave been developed and some have been specifically written for humanoid robots.

OpenHRP3 (Open Architecture Human-centered Robotics Platform version 3) is a simulatorand motion control library for humanoid robots developed at the National Institute of Ad-vanced Industrial Science and Technology [2]. This is a free tool that can simulate the directdynamics of open and closed chains of multibody systems. It includes a library of position,force/torque, vision, and inclination sensors. These are essential for developing control sys-tems for the robots. The software also includes simple controllers for locomotion, balancing,a walking pattern generator as well as collision detection and avoidance schemes. Advancedcontrollers can be developed by the users. Besides standard tools for plotting simulation re-sults, OpenHRP has a 3D visualization interface to display the simulation. The software de-veloped in OpenHRP can be directly used in the HRP humanoid robots but this is not possiblefor other humanoid robots. The main reason is that several features in OpenHRP cannot bemodified by the user. For example the actuator models are fixed, and compliance in the robotjoints or the robot links cannot be added to the model. Furthermore, there is very little docu-mentation available for OpenHRP and since this is a free tool there is also no support from thedevelopers.

Other simulators have been developed by Honda, Sony, Fujitsu for specific humanoid robots(for ASIMO, QRIO and HOAP). Commercial software packages like RoboWorks, Webots,Adams, are useful for modelling multibody systems, but lack generality for modelling actua-tors, sensors, ground contacts and impacts. None of these packages provide full control overall the features that we are interested.

To overcome the limitations in OpenHRP and existing simulators, [6] started developing anew dynamic simulator. This simulator will be tested in Archie, a humanoid robot developedat the Technical University of Vienna and the University of Manitoba. However, the simulatorcould also be used for different robots including robot arms and other mechanical systems.One important feature to be added in this simulator is the capability of hardware acceleratedcalculations. This would substantially reduce computational times. The developer also aimsto provide users with full control to add elements and customize the simulator to suit theirown applications. Currently Reichenbach’s simulator is not widely available and is still underdevelopment.

4

2. Robotran-Matlab Simulator for Control of Compliant Humanoid

Figure 2.1.: Robotran interface for data input.

Table 2.1.: Robotran featuresGenerate symbolic equations for dynamics and kinematics (planar/3D)Models for open and closed chainsModels can be linearized around any operating pointEquations are available in Matlab syntax and can use Matlab toolboxes for further analysisActuator dynamics, including compliance are added in MatlabGround impact models can be added in Robotran or MatlabA simple graphical user interface for data inputModel can be converted to match hardware sensors and actuators polaritiesLinear/angular positions, velocities, accelerations, orientations and Jacobians of any chosenpoints of the multibody system

5


Humanoid simulators have also been developed in Matlab, Simulink and SimMechanics forSilo2 [4]. In this approach modelling different types of actuators including compliance in therobot drive system is quite simple. Also a comprehensive set of toolboxes is available forthe design of advanced control systems. The rigid multibody modelling is essentially imple-mented in SimMechanics which also has tools for linearization and 3D visualization. Oneproblem using SimMechanics is that simulations become slow as the complexity of the sys-tem grows, particularly when schemes to detect ground contacts or impacts (event detection)are included in the simulation. Moreover, adding a good model of non-linear friction willsubstantially increase the simulation run-time. An alternative for rigid multibody modellingis to use Robotran [7] in place of SimMechanics. This has some advantages compared withSimMechanics. Robotran generates symbolic models rather than numerical models and thisimproves the numerical accuracy and speed of simulations. Furthermore, Robotran also has agraphical user interface which simplifies entering the robot data. Body parameters, e.g. mass,inertia, location of centre of mass and geometry, are entered for each body by filling the cor-responding information as shown in Fig. 2.1. The overall multibody system configuration isconstructed by simply drawing bodies and the corresponding joints connecting them. Figure2.2 shows a 10 degree of freedom model for the legs of the Compliant Humanoid.

2.2. Robot and Actuator Dynamics

This technical note considers adding the actuator dynamics to the equations of motion of thecompliant humanoid robot. It is assumed that the equations of the n-link robot dynamics aregiven in terms of relative joint angles θ and relative joint velocities θ . We consider the casewhen the coupling between the actuators and the robot joints includes the compliance in thedrive train. The drive compliance is modelled as an ideal torsional spring in parallel with anideal damper. The equations of motion are

M (θ) θ +N(θ, θ)θ + P (θ) = Bs(θm − θ) +Ks(θm − θ) (2.1)

Jmθm +Bmθm +Bs(θm − θ) +Ks(θm − θ) = Tm (2.2)

Here θm, θm and θm are n-dimensional vectors denoting motor position, velocity and accelera-tion reflected to the robot joints. Tm denotes the motor torque vector. The actuator coefficientsJm, Bm, Bs and Ksare all diagonal matrices

Jm = diag(Jmi), Bm = diag(Bmi)

Bs = diag(Bsi), Ks = diag(Ksi)

The motor torque Tm is proportional to the motor current Im

6


Figure 2.2.: Robotran graphical editor: Compliant Humanoid legs.

7


Tm = KtIm, (2.3)

Where Kt is the motor torque constant matrix

Kt = diag(Kti)

In the case of a current controlled motor the amplifier bandwidth is typically larger than 1 KHzand the robot-actuator dynamics can be approximated by equations (2.1), (2.2) and (2.3). Inthe case of a voltage controlled motor the current dynamics are given by

LmIm +RmIm +Kbθm = Vm

Where Vm denotes the applied motor voltages, Lm is the inductance matrix, Rm is the resis-tance matrix and Kb is the back emf motor matrix

Lm = diag(Lmi), Rm = diag(Rmi), Kb = diag(Kbi)

In most cases Lm ≈ 0 (fast electrical dynamics) and the motor torque can be approximated by

Tm = KtR−1m Vm −KtR

−1m Kbθm (2.4)

In this case the robot-actuator dynamics are given by equations (2.1), (2.2) and (2.4). Notethat for a voltage controlled motor equation (2.4) introduces additional damping in the actu-ator dynamics. The above description assumes that one motor/gearbox drives a single joint.However, two torso joints of the iCub (lateral and forward motions) are driven by a differen-tial drive unit with two motor/gearbox assemblies. In this case the sum of the motor torquesproduces front-back movement while the torque difference causes lateral motion of the torso.The right hand side of equation (2.1) needs to be modified to take this into account.

2.3. Actuator Models of the Compliant HumanoidPlatform

This technical note introduces several models for the Compliant Humanoid actuators. Themodels do not include the motor electrical dynamics. It is also assumed that translational androtational dynamics are uncoupled.

Drive: motor, gearbox and load inertia

The simplest model including the gearbox compliance is (in Fig. 2.3),

8


Table 2.2.: Description of actuator parametersJmi (kgm2) total inertia of ith drive reflected to ith joint. This should

include the motor, gearbox, encoders, shafts and couplingsKsi

(Nm/rad)equivalent drive stiffness reflected to joint. This shouldinclude the gearbox, shafts and couplings

Bsi

(Nms/rad)drive damping. Most likely this needs to be estimated fromexperimental tests.

Bmi

(Nms/rad)total viscous friction coefficient of the ith drive reflectedto the ith joint. This should include viscous friction of themotor, gearbox and bearings

Kti (Nm/A) ith motor torque constant reflected to the ith jointLmi (H) ith motor winding inductanceRmi (Ohms) ith motor winding resistanceKbi (Vs/rad) ith motor back emf constant

Figure 2.3.: Dynamic model of the Compliant Humanoid stiff joint.

9


Table 2.3.: Gearbox parameters (gearboxes GR or R)Harmonic DriveCSD-14-100-2A-GR

Harmonic DriveCSD-17-100-2A-GR

Inertia at gearbox inputkgm2

2.1e− 6 5.4e− 6

Stiffness at gearboxoutputNm/rad

0.40e4 Tload < 2Nm0.44e4 2 ≤ Tload ≤ 6.9Nm0.61e4 Tload > 6.9Nm

0.84e4 Tload < 3.9Nm0.94e4 3.9 ≤ Tload ≤ 12Nm1.30e4 Tload > 12Nm

Reduction ratio(Circular spline fixed)

−100 −100

Reduction ratio(Flexispline fixed)

101 101

No load starting torqueNm

2.4e− 2 3.3e− 2

No load backdrivingtorque Nm

3.1 4.1

Backlash(arcmin)

< 2 < 2

Viscous friction Can be estimated from manu-factures curves and lubricant

Can be estimated from manufac-tures curves and lubricant

Maximum input speedand average speed limit

Depend on lubricantLower values for grease

Depend on lubricantLower values for grease

Peak output torquelimitNm

31 55

Periodic peak outputtorque limit Nm

19 37

Average torque limitNm

7.7 27

Rated torqueNm

5.4 16

10


Table 2.4.: Motor parameters (A and B winding)KollmorgenRBE-00513

KollmorgenRBE-01210

KollmorgenRBE-01211

Inertia kgm2 4.31e− 7 5.15e− 6 8.47e− 6Viscous dampingNms/rad

8.0978e− 6 8.7663e− 6 1.9958e− 5

Max static fric-tion Nm

8.0e− 3 1.2e− 2 1.5e− 2

Max peak to peakcogging torqueNm

6.4e− 3 2.9e− 3 4.6e− 3

Motor windingtorque constantNm/AB winding(A winding)

2.86e− 2(2.26e− 2)

3.28e− 2(2.36e− 2)

6.57e− 2(4.10e− 2)

Motor windingback emf constantVs/radB winding(A winding)

2.86e− 2(2.26e− 2)

3.28e− 2(2.36e− 2)

6.57e− 2(4.10e− 2)

Motor windingresistance OhmsB winding(A winding)

1.75(1.11)

1.38(0.698)

1.75(0.664)

Motor windinginductance HB winding(A winding)

5.5e− 4(3.4e− 4)

5.4e− 4(2.8e− 4)

8.3e− 4(3.2e− 4)

Peak torque Nm 0.230 0.342 0.806Peak current AB winding(A winding)

8.26(10.4)

10.6(15.0)

10.6(20.0)

Continuous stalltorque Nm

0.0854 0.115 0.223

Continuous stallcurrent AB winding(A winding)

3.28(4.16)

3.89(5.41)

3.63(5.81)

11


Jmθm +Bmmθm +Bm(θm − θL) +Km(θm − θL) = Tm (2.5)

JLθL +BLLθL = Bm(θm − θL) +Km(θm − θL) + TL (2.6)

Here θm, θm and θm denote motor position, velocity and acceleration reflected to the output ofthe gearbox, Tm denotes the motor torque and TL is a disturbance torque. The load position,velocity and acceleration are θL, θL and θL respectively. The actuator parameters are Jm,Bmm, Bm, Km, JL and BLL. Equations (2.5)-(2.6) can be written in matrix form

[Jm 00 JL

] [θmθL

]+

[Bmm +Bm −Bm

−Bm BLL +Bm

] [θmθL

]+

[Km −Km

−Km Km

] [θmθL

]=

[TmTL

](2.7)

Note that the damping matrix has a particular structure: a diagonal matrix plus a symmetricmatrix. The symmetric damping matrix represents a damper Bm in parallel with the stiffnessKm.

Drive: motor, gearbox, cable drive and load inertia

Figure 2.4.: Dynamic model of the Compliant Humanoid compliant joint (SEA or cable drive).

An additional equation for the cable drive wheel is added to the previous model. The cablestiffness is also included in the model (in Fig. 2.4). The equations of motion for zero dampingare

Jmθm +Km(θm − θc) = Tm (2.8)

Jcθc +Kcrc(rcθc −RLθL) = Km(θm − θc) (2.9)

JLθL = KcRL(rcθc −RLθL) + TL (2.10)

The matrix equation for this case is

12


Jm 0 00 Jc 00 0 JL

θmθcθL

+

Km −Km 0−Km Km +Kcr

2c −KcrcRL

0 −KcrcRL KcR2L

θmθcθL

=

Tm0TL

(2.11)

For the non-zero damping we may add a term to the right hand side of equation (2.11):

Bmm 0 0

0 Bcc 00 0 BLL

+ α

Km −Km 0−Km Km +Kcr

2c −KcrcRL

0 −KcrcRL KcR2L

θm

θcθL

here α ≥ 0 is usually a small value (this introduces damper in parallel with each spring inthe model). When the wheel inertia is sufficiently small (Jc ≈ 0) the model can be reducedby using the equivalent drive stiffness Kme = KmKcr

2c/(Km + Kcr

2c ) (Km in series with

Kcr2c ). The equations for the reduced model are

Jmθm +Kme[θm − (RL/rc)θL] = Tm (2.12)

JLθL = Kme(RL/rc)[θm − (RL/rc)θL] + TL (2.13)

The corresponding matrix equation is

[Jm 00 JL

] [θmθL

]+

[Kme −KmeRL/rc

−KmeRL/rc Kme(RL/rc)2

] [θmθL

]=

[TmTL

](2.14)

Differential drive: 2 motors, 2 gearboxes, 2 cable drives and load inertia

Equations of motion for zero damping are

Jm1θm1 +Km1(θm1 − θ1) = Tm1 (2.15)

J1θ1 +K1r1(r1θ1 −RaθLa −RbθLb) = Km1(θm1 − θ1) (2.16)

JLaθLa = K1Ra(r1θ1 −RaθLa −RbθLb) +K2Ra(r2θ2 −RaθLa +RbθLb) + TLa (2.17)

JLbθLb = K1Rb(r1θ1 −RaθLa −RbθLb)−K2Rb(r2θ2 −RaθLa +RbθLb) + TLb (2.18)

J2θ2 +K2r2(r2θ2 −RaθLa +RbθLb) = Km2(θm2 − θ2) (2.19)

Jm2θm2 +Km2(θm2 − θ2) = Tm2 (2.20)

13


The corresponding matrix equation is in Appendix A.

A reduced model can be derived setting J1 = 0, J2 = 0 in (2.16) and (2.19). Solving thealgebraic equations for θ1 and θ2 gives

θ1 =1

Km1 +K1r21[Km1θm1 +K1r1(RaθLa +RbθLb)] (2.21)

θ2 =1

Km2 +K2r22[Km2θm2 +K2r2(RaθLa −RbθLb)] (2.22)

Substituting θ1 and θ2 in (2.15), (2.17), (2.18) and (2.20) yields

Jm1θm1 +Kme1[θm1 − (Ra/r1)θLa − (Rb/r1)θLb] = Tm1 (2.23)

JLaθLa = Kme1(Ra/r1)[θm1 − (Ra/r1)θLa − (Rb/r1)θLb]+Kme2(Ra/r2)[θm2 − (Ra/r2)θLa + (Rb/r2)θLb] + TLa

(2.24)

JLbθLb = Kme1(Rb/r1)[θm1 − (Ra/r1)θLa − (Rb/r1)θLb]−Kme2(Rb/r2)[θm2 − (Ra/r2)θLa + (Rb/r2)θLb] + TLb

(2.25)

Jm2θm2 +Kme2[θm2 − (Ra/r2)θLa + (Rb/r2)θLb] = Tm2 (2.26)

where

Kme1 = Km1K1r21/(Km1 +K1r

21), Kme2 = Km2K2r

22/(Km2 +K2r

22).

Assuming the cable stiffness is infinite yields the kinematic equations for the cable drive

r1θ1 = RaθLa +RbθLb, r2θ2 = RaθLa −RbθLb.

2.4. Waist Differential Drive Model with Yaw Rotation

Throughout this technical note it is assumed that translational and rotational dynamics areuncoupled and that all motor parameters have been reflected to the output of the harmonicdrive gearboxes.

Differential drive: 3 motors, 3 gearboxes, 3 cable drives and load inertia

When the cable drive wheel inertias are sufficiently small the equations of motion for zerodamping are

Jm1θm1 +Kme1[θm1 − (Ra/r1)θLa − (Rb/r1)θLb] = Tm1 (2.27)

14


Table 2.5.: GlossaryJm, Jm1, Jm2

(kgm2)Total drive inertia reflected to output of gearbox. Thisshould include the motor, gearbox, encoders, shaftsand couplings

Jc, J1, J2(kgm2)

Wheel inertia for cable drive

JL, JLa, JLb

(kgm2)Load inertia

Km, Km1,Km2

(Nm/rad)

Total drive stiffness at output of gearbox. This shouldinclude the gearbox, shafts and couplings.

Kc, K1, K2

(N/m)Cable stiffness. Can be calculated from cross sectionarea A, effective cable length L and Young’s modulusE: K = AE/L

rc, r1, r2(m)

Wheel radius for cable drive

RL, Ra, Rb

(m)Radius for cable drive output

Tm, Tm1, Tm2

(Nm)Motor torque

TL, TLa, TLb

(Nm)Load disturbance torque

θm, θm1, θm2

(rad)Motor angular position

θc, θ1, θ2(rad)

Wheel angular position for cable drive

θL, θLa, θLb

(rad)Load angular position

15


JLaθLa = Kme1(Ra/r1)[θm1 − (Ra/r1)θLa − (Rb/r1)θLb]+Kme2(Ra/r2)[θm2 − (Ra/r2)θLa + (Rb/r2)θLb] + TLa

(2.28)

JLbθLb = Kme1(Rb/r1)[θm1 − (Ra/r1)θLa − (Rb/r1)θLb]−Kme2(Rb/r2)[θm2 − (Ra/r2)θLa + (Rb/r2)θLb] + TLb

(2.29)

Jm2θm2 +Kme2[θm2 − (Ra/r2)θLa + (Rb/r2)θLb] = Tm2 (2.30)

Jm3θm3 +Kme3[θm3 − (Rc/r3)θLc − (D3/r3)θLb] = Tm3 (2.31)

JLcθLc = Kme3(Rc/r3)[θm3 − (Rc/r3)θLc − (D3/r3)θLb] + TLc (2.32)

Here Kme1, Kme2 and Kme3 denote the equivalent stiffness at the output of the harmonicdrive gearboxes. A glossary of symbols is given in Table 2.6.


21)


22)


23)

Assuming the stiffness is infinite yields the kinematic equations for the differential drive

r1θm1 = RaθLa +RbθLb

r2θm2 = RaθLa −RbθLb

r3θm3 = RcθLc +D3θLb

In the case of a voltage controlled motor the torques Tm1, Tm2 and Tm3 can be written interms of the applied motor voltage. In most cases the motor inductance is ≈ 0 (fast electricaldynamics) and the motor torque can be approximated by

Tm1 = Ng1Kt1R−1m1Vm1 −N2

g1Kt1R−1m1Kb1θm1 (2.33)


g2Kt2R−1m2Kb2θm2 (2.34)


g3Kt3R−1m3Kb3θm3 (2.35)

The matrix representation of the differential drive equations (2.27)-(2.35), including dampingand viscous friction, is

Jθ +Bθ +Kθ = GV + Td (2.36)

Where

θ =[θm1 θLa θLb θm2 θm3 θLc

]T

16


(a) (b)

(c)

Figure 2.5.: Open loop frequency response of differential drive with full decoupling

17


Figure 2.6.: Open loop frequency response of differential drive with partial decoupling λ = 0.

Td =[

0 TLa TLb 0 0 TLc

]TV =

[V1 V2 V3

]TThe corresponding matrices J , B, K and G are given in Appendix A.

Figure 2.5 shows the open loop frequency response of the differential drive system with fulldecoupling for the case when both drives are identical. For V =

[V1 0 0

]T, both load

b and load c velocities should be zero. For V =[

0 V2 0]T

, load a velocity should bezero and ideally load c velocity should also be zero but this can only be achieved at very lowfrequencies. For V =

[0 0 V3

]Tloads a and b have zero velocities and load c moves

(also the corresponding motor).

Figure 2.6 shows the frequency response for V =[

0 V2 0]T

with the decoupling gainλ set to zero. At low frequencies the velocity of load c is almost identical to that of load b.However as the resonant frequency is approached the velocity of load c becomes larger thanthat of load b.

Both figures clearly show the effect of the compliance in the drives. The amplitude plots showresonances for the load velocities (approximately 6, 6.2 and 6.6 Hz) and also show the corre-sponding anti-resonant frequencies for the motors. The resonant / anti-resonant frequenciesdepend on the stiffness values Kme1, Kme2, Kme3 and the load inertias. In general the closedloop bandwidth that can be achieved with simple controllers is around one half (or less) of theresonant frequency. The resonant frequency can be increased by reducing the load inertia orincreasing the stiffness in the drive train. For the differential drive the cable drive stiffness isdominant since it is much lower than the gearbox stiffness.

18


Table 2.6.: GlossaryJm1, Jm2, Jm3

(kgm2)Total drive inertia reflected to output of gearbox. Thisshould include the motor, gearbox, encoders, shaftsand couplings

JLa, JLb, JLc

(kgm2)Load inertia

Km1, Km2,Km3

(Nm/rad)

Total drive stiffness at output of gearbox. This shouldinclude the gearbox, shafts and couplings.

K1, K2, K3

(N/m)Cable stiffness. Can be calculated from cross sectionarea A, effective cable length L and Young’s modulusE: K = AE/L

r1, r2, r3(m)

Wheel radius for cable drive input

Ra, Rb, Rc

(m)Radius for cable drive output

Tm1, Tm2, Tm3

(Nm)Motor torque

TLa, TLb,TLc

(Nm)Load disturbance torque

θm1, θm2, θm3

(rad)Motor angular position

θLa, θLb, θLc

(rad)Load angular position

Ng1, Ng2, Ng3 Harmonic Drive gearbox reductionKt1, Kt2, Kt3

(Nm/A)Motor torque constant

Kb1, Kb2, Kb3

(Vs/rad)Motor back emf constant

Rm1, Rm2,Rm3

(Ohms)

Motor armature resistance

Vm1, Vm2, Vm3

(Volt)Motor input voltage

cm1, cm2, cm3

(Nms/rad)Motor viscous friction

cLa, cLb, cLc

(Nms/rad)Load viscous friction

D3 = r3(m)

Distance to model how θLb influences θLc

λ Decoupling gain for open loop model

19


2.5. ROBOTRAN: Symbolic Multibody Modelling

Robotran is a symbolic multibody modelling and simulation software package developed at theUniversite Catholique de Louvain in Belgium. The software is based on Matlab and Simulinkand provides a user-friendly environment. The main features of Robotran are summarizedin Table 2.1. The Robotran software package has three main components, the graphical userinterface (MBsysPad), a symbolic equation generator (MBsysTran) and an interface betweenRobotran and Matlab (MBsysLab). The user is provided with the graphical and Matlab inter-faces, while the symbolic equation generator is accessed on line via the Robotran web server.The symbolic equations are generated in a few milliseconds and the user receives a set of Mat-lab M-files with the symbolic equations. A 3D visualization/animation tool is also providedin MBsysPad, Fig. 2.7, but if the user prefers he can access similar tools in Matlab, Fig. 2.8.The Robotran software is free to use for simple systems (up to 5 joints). For more complexsystems (20-25 joints) and academic research the software is still free but requires registration.Documentation on how to use the software is limited and can be downloaded from the Robo-tran website www.robotran.be including some examples. There is also some on-line help inthe website. More detailed examples can be found in [5,7] has developed human body modelsin Robotran for biomechanics research.

Robotran has a number of features that are essential for modelling humanoid Robots. It gener-ates symbolic equations for dynamics and kinematics of open and closed chains. The symbolicmodels are generated as a collection of Matlab M-files. Furthemore, Robotran provides M-files where the user can define additional constraints/derivatives, as well as external link andjoint forces/torques. This is particularly useful for introducing compliance and damping in themultibody system as well as compliance in the robots drive system. Also these features canbe used to model ground reaction forces. Robotran can carry out linearizations around equi-librium points and modal analysis of the corresponding linear multibody system. Robotranprovides ideal sensors for obtaining positions, orientations (rotation matrices), linear/angularvelocities and accelerations of any point in the multibody system. In addition, for each sensorplaced on a body, the user can also extract the relevant Jacobians.

2.6. A ROBOTRAN-MATLAB Simulator for theCompliant Humanoid

In this section we describe the enhancements that have been developed to integrate actuatormodels into the symbolic equations generated by Robotran for the compliant humaonid de-veloped within AMARSI. For humanoid robot locomotion it is also necessary to take intoaccount the hybrid nature of the system, i.e. changing between open and closed chain mod-els (single/double support phases) as well as the modelling of ground impacts. Moreover, byintroducing suitable joint coordinate transformations, we describe how the Robotran modelsbecome more realistic and can be used to map simulation results directly to the CompliantHumanoid.

20


Figure 2.7.: Robotran 3D visualization.

Figure 2.8.: Robotran 3D visualization of the Compliant Humanoid legs in Matlab.

21


2.6.1. System Modelling

The equations of motion of the lower body derived in Robotran are based on the recursiveNewton-Euler formalism and the constraint forces are introduced via the Lagrange multipliersmethod. The equations of motion take the form

M (θ) θ + c(θ, θ, f, τ

)= Q(θ, θ, θm, θm) + JT (θ)λ (2.37)

h (θ) = 0 (2.38)

h(θ, θ)

= J(θ)θ = 0 (2.39)

h(θ, θ, θ

)= J(θ)θ + J θ(θ, θ) = 0 (2.40)

Equations (2.37)-(2.40) are generated by the MBsysTran module in Robotran. In the case ofan open chain model the Lagrange multipliers,λare zero and (2.38)-(2.40) can be eliminated.The user has complete freedom to specify the external link forces and torques (f, τ ) as wellas the external joint forces and torques Q(θ, θ, θm, θm). For the Compliant Humanoid theexternal driving torques are generated by compliant joints with damping. Since the robot waistjoints are driven by a differential drive system, it is also convenient to split the drive torquesQ(θ, θ, θm, θm) in two blocks. Equation (2.37) is generated so that the last three equationscorrespond to the waist joints (sagittal, lateral and yaw). Defining

Q(θ, θ, θm, θm) =

[Qlegs(θlegs, θlegs, θmlegs, θmlegs)

RwaistQwaist(θwaist, θwaist, θmwaist, θmwaist)

]

Qm(θ, θ, θm, θm) =

[Qlegs(θlegs, θlegs, θmlegs, θmlegs)

Qwaist(θwaist, θwaist, θmwaist, θmwaist)

](2.41)

where

Qlegs(θlegs, θlegs, θmlegs, θmlegs) = Kslegs(θmlegs−θlegs)+Bslegs(θmlegs− θlegs) (2.42)

and

Qwaist(θwaist, θwaist, θmwaist, θmwaist) =

Kswaist(θmwaist −Rwaistθwaist) +Bswaist(θmwaist −Rwaistθwaist) (2.43)

The set of second order differential equations for the drives are

Jmθm +Bmθm = Tm −Qm(θ, θ, θm, θm) (2.44)

The matrices Rwaist and Rwaist depend on the kinematics of the differential drive (see Ap-pendix for details) and are given by

Rwaist =

1 1 01 −1 00 1 40/22

Rwaist =

1 1 01 −1 00 0 40/22

22


When the joint torques are generated by motors, the user needs to add the relevant differentialequations (2.44) in a user Derivatives M-file. Additional equations may be required dependingon the type of actuators (electric, hydraulic, pneumatic). For the Compliant Humanoid theactuators are voltage controlled electric motors, and neglecting the electrical dynamics gives

Tm = KtR−1m Vm −KtR

−1m Kbθm (2.45)

The parametersKt,Kb are equivalent torque and bemf motor constants (motor parameters re-flected to the joint axis); Rm represents the motor armature resistance and Vm is the appliedmotor voltage.

The MBsysLab module in Robotran provides a set of M-files to carry out a number of tasks:linearization and modal analysis; direct and inverse dynamic calculations (required for findingequilibrium points); coordinate partitioning for closed chain models; direct kinematics. Forinverse kinematics the user can solve this problem by selecting a suitable numerical method(for example Newton-Raphson) and implement this in Matlab. For the Compliant Humanoidwe have developed an M-file using the sensor data from Robotran and solved the inverse kine-matics problem for a position of the Robot centre of mass as well as the position/orientationof swing leg foot. The user also has the freedom to select other methods for closed loopchains and direct dynamics calculations. In this case the user can develop its own M-files andonly call the relevant functions provided by Robotran containing the symbolic equations. Forexample, solving (2.37) and (2.40) we can write an expression for the Lagrange multipliers

λ = −[JM−1JT

]−1 ••{JM−1

(Q(θ, θ, θm, θm)− c

(θ, θ, f, τ

))+ J θ(θ, θ)

} (2.46)

Substituting (2.45) in (2.37) yields

M (θ) θ = Q(θ, θ, θm, θm)− c(θ, θ, f, τ

)−JT (θ)

[JM−1JT

]−1 ••{JM−1

(Q(θ, θ, θm, θm)− c

(θ, θ, f, τ

))+ J θ(θ, θ)

} (2.47)

We have written a set of M-files for all the actuators of the Compliant Humanoid (2.41)-(2.45).The M-files are then combined with the Robotran model (2.37)-(2.40) to produce the overallmodel of the robot. For linear control system design a similar approach is used except that thelinearized Robotran model replaces (2.37)-(2.40). Discrete time control systems have beendesigned in the Robotran-Matlab simulator using LQR optimal control and state observers.These controllers have been implemented and tested on the real hardware. Initial tests werecarried out on the legs and the upper body of ICub while the robot was off the ground. Morerecently a balancing controller was tested on the Compliant Humanoid to keep an uprightposture while the robot was on the ground (double support phase). The results of these testshave partially validated the modelling approach described in this section.

23


2.6.2. Ground Impacts and Exchange of Support Leg

Humanoid locomotion requires switching between closed and open chain models (exchangebetween single and double support phases). Inevitably this switching introduces ground im-pacts and the relevant models can be added to (2.37)-(2.41) directly in Robotran or Matlab. Asimple model for ground impacts takes the form

θ+ = Φ(θ−) (2.48)

Here, θ−and θ+ denote the joint velocities before and after the impact. Furthermore, whenthe role of support and swing legs is reversed we also need to swap the joint positions andvelocities in the model (also motor positions and velocities if applicable). The reason for thisis that the Robotran equations assume that the support leg does not change

θastanceθaswing

θastanceθaswing

=

θbswing

θbstanceθbswing

θbstance

(2.49)

Here the superscripts a and b correspond to the event after and before the exchange of support.

Simulation of a full locomotion cycle therefore requires stopping the simulation when an im-pact is detected, carrying out the relevant calculations, swapping the role of legs in the modeland re-starting the simulation. In this manner we can achieve a realistic simulation for severallocomotion cycles. More sophisticated event detection schemes as those shown in Fig. 6 canalso be implemented in Matlab.

2.6.3. Robotran Model Transformations

When building a model in Robotran, it is important to understand how the reference framesare chosen as well as the joint displacements convention used in Robotran. This is particularlyimportant for mapping results in Robotran to the real hardware and developing control systemsthat can be ported directly to the real system. When drawing the multibody system in Robo-tran all the body reference frames must be aligned. This is called “the standard configuration”and corresponds to zero displacements. For the Compliant Humanoid the example shown inFig. 2.2, the standard configuration corresponds to the upright position. In this example thereference frame XYZ is chosen so that the positive x-axis points in the direction of forwardwalking, the positive z-axis points upwards and the positive y-axis is chosen to complete anorthogonal right handed coordinate system Fig. 2.7. In Robotran all joint displacements aregiven as relative motions and the right hand rule convention determines the direction of posi-tive rotations. Absolute displacements can be computed from the set of relative displacementsand the corresponding time derivatives. For the particular case of the Compliant Humanoid,the joint angles measurements for the stance leg are consistent with the convention in Robo-tran. However, the joint angles of the swing leg have the opposite direction to the Robotranconvention. It is important to be aware of these differences, particularly when developing

24


walking trajectories and control systems. To “convert” the Robotran model so that the anglesin the model correspond to the angles in the robot we can define a suitable transformation.The transformation is quite simple and easy to implement. The relation between the relativejoint angles in Robotran and the Compliant Humanoid is given by

θR = Wθrobot (2.50)

where θR denotes the joint angles in Robotran, θrobot are the angles in the Compliant Hu-manoid and W is a square invertible matrix

W =

[I5×5 05×5

05×5 −I5×5

](2.51)

here I5×5, 05×5 denote 5 × 5 identity and zero matrices respectively. When the actuators aremodelled a similar transformation is required for the motor angles. In this manner, we can“transform” the Robotran model into a model that matches the real system sensors readings.The transformation W and the inverse transformation can be incorporated into the relevantsymbolic equations in the M-files generated by Robotran. A similar procedure can be imple-mented to cater for differences in the actuators signal polarities.

2.7. ROBOTRAN-MATLAB Simulator Demo Models

Two case studies for the Robotran-Matlab simulator have been developed.

The first case is based on a compass gait walker [3]. This is a simple two legged un-actuatedsystem walking down a slope with curved feet and including a model for ground impacts.To model the slope in Robotran, the gravity vector is defined as a vector with vertical andhorizontal components (along the slope direction). The ground impact model equations wereinitially derived in a terms of a different angle convention to that of the symbolic model inRobotran. In this case we need to implement a transformation similar to that described inthe previous subsection. After converting the Robotran angles (and angular velocities), theimpact model equations are used to obtain the joint velocities after the impact. The result isthen converted back to the Robotran angle convention and the legs are swapped using (2.49).The Compass gait demo simulator will run for several steps and the simulation results aredisplayed in a Matlab animation.

The second case study is the Compliant Humanoid simulator. The current version is only for10 degrees of freedom (the two legs). The model parameters (masses, inertias and geometry)were obtained from a CAD package. This is the first simulator for the Compliant Humanoidthat includes the dynamic equations of motion. The relevant actuator models for the legs areincluded as Matlab M-files. The Robotran model in the demo is for the single support phase(stance right leg, swing left leg). This model is unstable and a stabilizing LQR controller isprovided in the demo. A Matlab file is also available for solving a simple inverse kinematicproblem and generate walking trajectories for the demo. As the project develops the softwarewill be updated. The goal is that the simulator will become a good representation of the

25


real system. Our current interest is to use the simulator for designing, testing and comparingdifferent control systems [1]. Users that are interested in methods for trajectory generationcan also use this model.

Figure 2.9.: Four event detection scheme.

26

3. AMARSi Humanoid Model for theOpenHRP Simulator

OpenHRP (Open Architecture Humanoid Robotics Platform) [2], is an open-source integratedsimulation platform that allows to simulate the dynamics and control of any humanoid roboticsystem. With this software we developed the dynamic model of the AMARSi CompliantHumanoid lower body in order to perform studies on humanoid locomotion (e.g. dancing,walking, running, etc.). At the moment the model accounts only for stiff joints ( we are cur-rently extending this) therefore it does not include the compliance which has been introducedin the AMARSi Humanoid prototype. The simulated lower body robot has distributed massesmade of up seven rigid segments, namely the thigh, calf, and foot for each leg, and the upperbody as a whole.

3.1. Using the OpenHRP Simulator for verifyinglocomotion trajectories for the lower body

This section describes a generic trajectory generation method using the preview control basedon a cart-table model [9,12,13,15]. This pattern generator produces joint trajectories of quasi-dancing and walking. The trajectories were generated off-line from simulations based on therobot dynamics. Based on the cart-table model, the preview controller was used to generatea number of quasi-dancing and walking trajectories. The control method was then verifiedin the dynamic simulator OpenHRP [2] using the developed model template of the underconstruction humanoid. Real experiments were also executed afterwrds to assess the validityof the robot model implemented in the OpenHRP simulator [10].

The flow chart of the trajectory generation algorithm used is summarized in Fig. 3.1. Thedesigned ZMP trajectories are the reference inputs for the preview controller. By using thedynamics of the cart-table model, the preview controller, which consists of two stages, gener-ates a centre of mass (COM) motion such that the resulting ZMP tracks the reference input. Inthe first control stage, the preview controller calculates the first multi-body ZMP based on thetranslational and rotational motion of each body segment solved using the inverse kinemat-ics. Hence, the ZMP tracking error caused by the difference between the single mass modeland the multi-body model is obtained. Therefore, in the 2nd stage, the controller feeds in thetracking error to further minimize the ZMP error.

27

3. AMARSi Humanoid Model for the OpenHRP Simulator

Figure 3.1.: Flow chart of overall scheme

3.2. Simulations using OpenHRP and Experiments inthe Prototype Lower body

3.2.1. ZMP Trajectory Planning for Quasi-Dancing and Walking

In this case we demonstrated the use of the OpenHRP simluation and lower robot model de-veloped to evaluate quasi-dancing trajectories. The quasi-dancing is considered as a periodicalmotion with both feet on the ground. Therefore, the ZMP on the ground form a closed form.We apply the equations listed below to produce such ZMP trajectories.

xZMP = Axsin(k · ω · t) + d (3.1)

yZMP = Aysin(ω · t), (3.2)

where k = 2, 4, · · · , ω = 2 · π ·m, m ∈ R+, and d = 0.02m is the offset from zero sagittalposition to foot center. Here the parameter k is the even number to obtain a closed form ZMP

28


Table 3.1.: Mass and inertia tensor parameters of the robotSegment Mass (kg) Inertia Tensor (kg ·m2)

Pelvis and upper body 5.540.0253 0 0

0 0.0085 00 0 0.0216

Thigh 2.77× 20.0144 0 0

0 0.0144 00 0 0.0007

Calf 2.47× 20.0154 0 0

0 0.0148 00 0 0.0012

Foot 0.63× 20.0005 0 0

0 0.0012 00 0 0.0009

path. Fig. 3.2(a) and 3.2(b) illustrates the ZMP dancing trajectories on the ground. The splinefunction smoothly transfers the ZMP from zero position to the start of dancing trajectory.

For walking, the gait parameters such as step length, gait cycle, foot lift height, and so forth arespecified. Firstly, the foothold are planned given step length. Secondly, the ZMP trajectory isplanned based on the time-line according to gait cycle time. The ZMP is located in the centerof the foot in the single support phase, ensuring a safety margin. Here, we set the doublesupport phase as 20% of the whole gait cycle, and the ZMP transition in double support phaseis continuously connected from one support foot to another by the spline function. Finally, theZMP trajectory which moves inside the support polygon is obtained, as shown in Fig. 3.2(c).

In Fig. 3.2, the red line is the reference ZMP, the green line is the multi-body ZMP calculatedafter the 1st control loop, and the blue line is the multi-body ZMP computed after the com-pensation of the 2nd control loop. It can be seen that the final obtained ZMP of the CompliantHumanoid is very close to the reference, remaining inside the support polygon defined by twofeet. Therefore, this motion is considered as stable. we then proceeded to dynamic simulationusing the OpenHRP and experiments with the lower body platform to verify the trajectories.

3.2.2. Simulation Experiments of Dancing and Walking

The generated trajectories were then used in the OpenHRP robot simulator [2]. Joint anglesand velocity trajectories were the reference for the joint tracking controllers. In quasi-dancing,parameters in (3.1) and (3.2) are: k = 2, 4, ω = 2π. Walking parameters are: step length0.10m, gait cycle 3s, step width 0.064m, foot lift height 0.03m.

Having verified the validity of the walking trajectories in the OpenHRP simulator using thelower body model preliminary walking experiments were carried out also in the real platform.Fig. 3.5 illustrates a series of dancing body posture snapshots. Fig. 3.6 show the snapshotsof front and side view of walking experiments. It demonstrates that Compliant Humanoid

29


(a) Dancing parameter k=2 (b) Dancing parameter k=4

(c) ZMP tracking of walking

Figure 3.2.: Reference and multi-body ZMP trajectories for dancing and walking

30


(a) Front view of dancing (b) Side view ofdancing

Figure 3.3.: Simulated dancing of the Compliant Humanoid using OpenHRP

(a) Front view of walking (b) Side view of walking

Figure 3.4.: Simulated walking of the Compliant Humanoid in OpenHRP

31


Figure 3.5.: Snapshots of the Compliant Humanoid dancing

(a) (b)

Figure 3.6.: Front view and side view of the Compliant Humanoid walking

32


was capable of replicating the reference trajectories with good fidelity showin good matchingbetwwen the simulated and experimental platform.

33

4. Impact Simulation and StiffnessTuning for a 4-DOF CompliantArm/Leg Robotic System

Impact simulation is used to analyze the stiffness by checking the peak torque of each jointduring the impact to make sure that the responded joint torques do not exceed the maximumtorque value the compliant actuator can withstand. Considering the integrated impact func-tion, parameter based torsion spring model and good dynamic analysis environment, Adamsis a good choice for the impact analysis of compliant manipulators. As there are countlessconfigurations in the whole workspace, you cannot simulate all of them to check. Thus, theapproach in this study is firstly to find the worst arm configuration in the workspace for eachjoint based on dynamic equations. Then, simulate this worst configuration in Adams to checkthe peak torques by giving appropriate stiffness. If the generated torque is safe for the jointsin this worst configuration, it will be safe for all other configurations when the robot joint ve-locities or the end-effector velocity are within a certain value used for the worst configurationsearching and impact. As shown in Fig. 4.1, the procedure for the impact simulation analysisis starting from the worst configuration identifying, which gives the configuration parametersfor building the Adams model and also supplies initial joint angle velocities and torques forthe impact simulation. In the impact simulation, peak torques of all joints can be checked andcandidate stiffness can be obtained. This work has been published in [11].

4.1. System Dynamics and Worst ImpactConfiguration for Joint 1 of the 4-DOF Arm/Legrobot

In this section the procedures described in the stiffness design section are applied for theanalysis of the 4-DOF complaint robotic system1. The compliant robotic system consistsof a 3-DOF shoulder/hip joint and 1-DOF elbow/knee joint as shown in Fig.4.2. The threerotational axes(z1, z2, z3) at the shoulder/hip are intersecting at one point. The upper linkconnects the 3-DOF joint with the 1-DOF joint, (z4) which represents the elbow/knee joint,and the forearm/shin as the end-effector. The wrist/ankle is not considered in this study. Fromnow on the text will refer only to the the robotic arm, however the analysis applies to the

1The 4-DOF compliant robotic system can be either a robotic arm or leg, 3-DOF shoulder/hip and 1-Dof elbow/knee.

34

4. Impact Simulation and Stiffness Tuning for a 4-DOF Compliant Arm/Leg Robotic System

Figure 4.1.: Flow chart for impact simulation.

35


robotic leg as well. A frame coordinate system o0x0y0z0 is set at the intersecting point at theshoulder with z0 collinear with the direction of joint z1 and y0 to the opposite direction ofgravity as in Fig. 4.2. vvv is the velocity of the end-effector and FFF is the impact force applied atthe end-effector.

Figure 4.2.: Kinematics model of the 4-DOF arm/leg.

The dynamic equations integrated with the impact model can be specified in terms of thesearching parameters and the worst configurations can be identified by following equations:

max τi(q1, q2, q3, q4, α1, α2, β1, β2)−7π/9 ≤ q1 ≤ 7π/9−5π/9 ≤ q2 ≤ 7π/9−π/2 ≤ q3 ≤ π/2−π/2 ≤ q4 ≤ 5π/180 ≤ α1, β1 ≤ π0 ≤ α2, β2 ≤ 2πn.nv < 0

(i = 1, 2, 3, 4) (4.1)

where qi (i =1, 2, 3, 4) are the joint angles, α1 and α2 represent the direction of end-effectorvelocity, and β1 and β2 describe the direction of the impact force. Here α1 and β1 are theangles between the directions and axis z0, α2 and β2 are the angles between the projectionof the directions on the x0o0y0 plane and axis x0. The direction of the impact force shouldbe within the opposite hemisphere of the velocity, indicating that the impact force should nothave positive element on the end-effector velocity direction and giving a constraint as n.nv

being negative.

The input parameters of the 4-DOF arm are listed in Table 4.1. The arm masses and linklengths and the joint angle ranges are taken from the iCub robot [8]. One more importantparameter is the magnitude of the end-effector velocity which is set as 1.5 m/s and the jointvelocities are obtained by inverse calculating using the manipulator Jacobian. Thus, based on

36


Table 4.1.: Input parameters of the 4-DOF arm/leg.Item Mass Length AccelerationUnit kg m rad/s2

Upperarm

2 0.2 N/A

forearm 2 0.2Joint 1 N/A 1Joint 2 N/A 2Joint 3 N/A 0.2Joint 4 N/A 1

equation (4.1) and the input parameters in Table 1 with the end-effector velocity as 1.5 m/s,the worst configurations of joint 1 can be obtained as

τw1(0.043, 0.58, 0.72,−1.08, 1.97, 0.66, 1.17, 0.65) (4.2)

where the unit for the angles is radian. The worst configuration is demonstrated in Fig. 4.3, inwhich the red line represents the direction of the impact force and the blue one is the directionof the end-effector velocity. Thus, joint 1 needs to support the highest joint torque to supportthe end-effector velocity of 1.5m/s in the direction shown by the blue line while an impactoccurs in the direction of the red line.

Figure 4.3.: Worst configuration for joint 1 in the 4-DOF arm/leg robot.

37


4.2. Impact Simulation and Stiffness Analysis for Joint1 of the 4-DOF arm/leg robot

Based on the result in equation (4.2) and the structure parameters in Table 4.1, a simulationmodel of the 4-DOF compliant arm corresponding to the worst configuration in Fig 4.3 is builtin the Adams as in Fig. 4.4, in which the first red cylinder is the shoulder base, the secondshort red cylinder with the green cylinder form the upper arm while the yellow cylinder is theforearm. The three rotational joints intersect at the intersecting point of the central lines of thetwo red cylinders. The elbow joint is perpendicular to the two central lines of the green andyellow cylinders.

Figure 4.4.: Simulation model for joint 1 of the 4-DOF arm/leg robot.

In Adams, you cannot give the end-effector velocity to calculate the joint angle velocities asthat in the worst configuration searching. In this case, the simulation procedure is to givethe initial joint angle velocities and joint torques to see the impact force and responded jointtorques. Thus, it is reverse to the worst configuration calculating. The initial input parametersfor the simulation can be obtained from the worst configuration calculation as shown in Table4.2, in which J1 corresponds to the worst configuration of joint 1 in Fig. 4.3. The jointvelocities are obtained by inverse kinematics from the end-effector velocity, and the inputjoint torques are the values needed to execute the motion of the arm in the configuration inFig. 4.3 to give the initial end-effector velocity as 1.5m/s and the joint angle accelerations inTable 4.1.

38


Table 4.2.: Input parameters for the simulation of joint 1 of the 4-DOF arm/leg robot.q1 q2 q3 q4 τ1 τ2 τ3 τ4

Unit rad/s rad/s rad/s rad/s Nm Nm Nm NmJ1 10 6.6234 -10 -10 20.6698 -7.7789 4.1519 5.0046

In the simulation, the impact force is set according to the direction of the red line in the worstconfiguration in Fig. 4.3 at the end of the forearm. The magnitude of the impact force ismodeled by the Hertz contact model which is an impact function in ADAMS. The stiffnessand damping coefficient of the impact model are set as 106N/m and 250 Ns/m respectively.As the maximum torque which the compliant actuator can withstand is 55Nm, the purposeof the simulation is to set different stiffness of the four compliant joints to ensure that thegenerated peak joint torques do not exceed the maximum level of 55Nm. After different trials,the following results are obtained and presented in Fig. 4.5. Figure 4.5(a) shows the changeof the end-effector velocity and impact force while Fig. 4.5(b) demonstrate the results of thejoint torques. In order to show the velocity change clearly, the period is only set from 0 secondto 0.3 second. It can be seen that the end-effector velocity (dashed line in blue) starts from1.5m/s and changes suddenly according to the impact force depicted by solid line in red. Thefour curves in Fig. 4.5(b) represent the torques of the four joints respectively with their peaktorques close to but under 55 Nm.

Corresponding to the results shown in Fig. 4.5, the joint stiffness for the four compliant jointis 687Nm/rad, 1432 Nm/rad, 2578 Nm/rad and 1547 Nm/rad respectively.

In the worst configuration in Fig. 4.3, the motion torque is 20.6698 Nm while the impacttorque is only 1.029Nm with impact force 10.927N. Thus the main part for the worst config-uration torque comes from the motion requirement. This may cause suspicion whether thisconfiguration is reasonable to be the worst as the impact force is normally much higher than10.927N. To further investigate this issue, constant impact forces are used in the worst config-uration identifying while the other input parameters are the same with those in Table 4.1 andthe end-effector velocity is 1.5m/s. The results show that the worst configuration is similarwith that in Fig. 4.3 when the constant impact force value is set below 22N and it becomesthe one shown in Fig. 4.6(a) if the constant impact force is more than 22N, up to 5000N andmore. In the configuration in Fig. 4.6(a), the main contribution (1199.98Nm) to the joint 1torque (1204.41Nm) is from the impact force (3000N, red line) which is perpendicular to joint1 axis (axis y). It can be also seen in Fig. 4.6(a) that the elbow (direction in green) is straightwith the upper arm and the forearm being in line. This also confirms that the impact forcedominate the result to have the longest arm from the force point to joint 1 axis to result in thehighest impact torque to joint 1.

Following the same procedure of previous analysis, a simulation model was set up in Adamsbased on the worst configuration depicted in Fig. 4.6(a). The simulation results are illustratedin Fig. 4.6(c). In this study, two cases have been analyzed. In case 1, the stiffness in J1 insection 3.3.2 is applied for the joints and the corresponded joint 1 torque is shown in Fig.4.6(c) in blue. It is obvious that by using the same stiffness, the peak torque of joint 1 in theworst configuration case shown in Fig. 4.6(a) is lower than 55Nm which is the peak torque of

39


(a) Impact force and end-effector velocity.

(b) Joint torques.

Figure 4.5.: Simulation results for joint 1.

40


(a) The worst configuration with constant im-pact force.

(b) The simulation model.

(c) Simulation results.

Figure 4.6.: A comparison configuration for joint 1.

41


Table 4.3.: Input parameters for the simulation.q1 q2 q3 q4 τ1 τ2 τ3 τ4

Unit rad/s rad/s rad/s rad/s Nm Nm Nm NmJ2 10 -

4.0564-10 10 8.5737 20.821 -2.0029 -

0.3926J3 10 -10 -10 -10 7.2031 -19.0867 11.7957 0.2108J4 -10 -10 -

0.59382.7815 -

0.58795.9208 -0.1545 10.703

joint 1 in J1 case in section 3.3.2, indicating that the J1 configuration in Fig. 4.3 is worse thanthis one in Fig. 4.6(a) for joint 1. In case 2 simulation, similar procedure as that in section3.3.2 is used to derive the maximum stiffness for all the four joints for the configuration shownin Fig. 4.6(a) which ensure that the generated joint torques remain under the maximum valueof 55Nm. The corresponded joint 1 torque is demonstrated in Fig. 4.6(c) by the red lineindicating peaks up to 55Nm. The final stiffness values for the four joints are: 3323 Nm/rad,2864 Nm/rad, 1432 Nm/rad and 3896 Nm/rad respectively. Comparing with those in section6.1, it can be observed that these values are higher than those of the J1 case, which proves thatJ1 configuration case in section 6.1 is the worse for joint 1.

4.3. Impact Simulation Analysis of the Other ThreeJoints in the 4-DOF Arm/Leg robot

Following the same procedure for the analysis of joint 1, the other three joints in the 4-DOFarm can be analyzed. Based on equation (4.1) and the input parameters in Table 4.1 withthe end-effector velocity as 1.5 m/s, the worst configurations of the other three joints can beobtained as

τw2(1.65,−0.88, 1.17,−1.31, 1.95, 6.09, 2.06, 1.17)τw3(0.72, 2.26, 1.04,−0.34, 2.04, 0.68, 1.37, 1.25)τw4(0.28,−0.011, 1.39,−0.65, 1.71, 3.37, 2.92, 1.32)

(4.3)

where the unit for the angles is radian. Using the parameters obtained in Eq. (4.3) and thestructure parameters in Table 4.1, the Adams models of the worst configurations of the threejoints can be built as shown in Fig. 4.7.

In the worst configuration identifying, the input parameters for the impact simulation can bealso obtained as in Table 4.3, in which Ji (i =2, 3, 4) corresponds to the worst configurationsof joint i as in Fig. 4.7.

Setting different stiffness of the four compliant joints to ensure that the generated peak jointtorques do not exceed the maximum level of 55Nm, the following results are obtained andpresented in Fig. 4.8, from which, it can be seen that by increasing the joint stiffness in each

42


(a) Joint 2. (b) Joint 3.

(c) Joint 4.

Figure 4.7.: Adams models of the other three joints.

43


Table 4.4.: Stiffness for the 4-DOF arm.k1 k2 k3 k4

Unit Nm/rad Nm/rad Nm/rad Nm/radJ1 687 1432 2578 1547J2 3151 286 2005 2864J3 716 544 744 573J4 1890 303 3151 544

case to appropriate values, the peak torques of the all the four joints are constrained close tobut within 55Nm.

(a) (b)

(c)

Figure 4.8.: Simulation results of the other three joints.

The final stiffness values of all the four joints derived from the simulation of the four worstconfiguration cases are listed in Table 4.4. It can be seen that joint 2 requires comparativelysmaller stiffness than other joints, which means joint 2 is more sensitive to impact forces inthese four configuration cases. By comparing the rows in Table 4.4, it is obvious that thestiffness of the four joints in the case of joint 3 worst configuration is relatively lower thanother cases. This can also be confirmed by the joint angle velocities in Table 4.3 as all the fourreach the maximum 10 rad/s and this configuration is sensitive to the impact.

In Table 4.4, the most important values are those underlined, which shows that the maximumstiffness for joint 1 to joint 4 should be 687 Nm/rad, 286 Nm/rad, 744 Nm/rad and 544 Nm/radrespectively. By setting the values lower than these numbers, the joint actuators will be safe

44


when there is an impact at the tip end with the end-effector velocity as high as 1.5 m/s. Theseunderlined values are in the diagonal positions in Table 4.4, indicating that Ji case is the worstconfiguration for corresponding joint i (i =1, 2, 3, 4).

45

A. Differential Drive Models in MatrixForm

The matrix coefficients in equation (2.36) are

J =

Jm1 0 0 0 0 0

0 JLa 0 0 0 00 0 JLb 0 0 00 0 0 Jm2 0 00 0 0 0 Jm3 00 0 0 0 0 JLc

K =

[K11 K12

]

K =

Kme1 −Kme1Ra

r1−Kme1

Rb

r10

−Kme1Ra

r1R2

a

(Kme1

r21+ Kme2

r22

)RaRb

(Kme1

r21− Kme2

r22

)−Kme2

Ra

r2

−Kme1Rb

r1RaRb

(Kme1

r21− Kme2

r22

)R2

b

(Kme1

r21+ Kme2

r22

)Kme2

Rb

r2

0 −Kme2Ra

r2−Kme2

Rb

r2Kme2

0 0 −Kme3(D3/r3) 00 0 Kme3(RcD3/r

23) 0

K12 =

0 00 00 00 0

Kme3 −Kme3(Rc/r3)−Kme3(Rc/r3) Kme3(Rc/r3)2

The damping matrix B is given by two terms, one containing viscous friction and back emfterms Bvand a second term containing damping due to the complianceBK . The contributionof BK is usually quite small and the dominant damping arises from back emf effects.

B = Bv +BK

46

A. Differential Drive Models in Matrix Form

Bv =

cm1 + N2

g1Kt1R−1m1Kb1 0 0 0 0 0

0 cLa 0 0 0 00 0 cLb 0 0 0

0 0 0 cm2 + N2g2Kt2R

−1m2Kb2 0 0

0 0 0 0 cm3 + N2g3Kt3R

−1m3Kb3 0

0 0 0 0 0 cLc

BK = αK

Where α is a small positive scalar (e.g, 1× 10−4 ≤ α ≤ 5× 10−3)

The gain matrix G can be expressed as a product of two matrices GT and Gdec. The matrixGT denotes the voltage to torque gain and the second matrix Gdec can be used to decouple thedrives motion, so that the first input voltage V1only affects the load inertia JLa and the secondinput voltage V2 only affects the load inertia JLb

G = GTGdec

GT =

Ng1Kt1R

−1m1 0 0

0 0 00 0 00 Ng2Kt2R

−1m2 0

0 0 Ng3Kt3R−1m3

0 0 0

Gdec =

1 1 01 −1 00 λ 1

The condition for exact decoupling is Ra

r1= Rb

r2= Ra

r2= Rb

r1this ensures that decoupling

occurs at all frequencies (provided drives are identical). For other cases decoupling is onlyapproximate and may be possible to achieve good decoupling at low frequencies.

The gain λ can be tuned so that the open loop response of the yaw motion θLc is unaffectedwhen moving θLb. The open loop“compensation” using λ will only be effective at low fre-quencies and λ is proportional to D3. In practice a closed loop controller should carry outthis compensation. The expression for Gdec is only an approximation for the case with equaldrives and Ra

r16= Rb

r26= Ra

r26= Rb

r1. It seems this is reasonably good in many cases but it is only

an approximation. The reason is that the dynamics are greatly affected by changes in theseparameters particularly the coupling between the drives and loads and achieving decouplingbecomes a dynamic problem.

47

References

[1] Dallali, H., Medrano-Cerda, G.A. and Brown, M., “A comparison of multivariable anddecentralized control strategies for robust humanoid walking,” UKACC InternationalConference on Control 2010, Humanoid Robotics special session,, Coventry, September7-10, 2003.

[2] Kanehiro, F., Hirukawa, H. and Kajita, S., “OpenHRP: open architecture humanoidrobotics platform,” International Journal of Robotic Research, Vol. 23, no. 2, pp. 155-165, 2004.

[3] Karssen, D. and Wisse, M., “Limit cycle analysis,” Tutorial, Dynamic Walking Confer-ence, Delft, The Netherlands, May 26-29, 2008.

[4] Ponticelli, R. and Armada, M., ”Vrsilo2: dynamic simulation system for the bipedrobot silo2,” Proceedings of the 9th International Conference on Climbing and Walk-ing Robots, Brussels, September, 2006.

[5] Raison, M., ”On the quantification of joint and muscle efforts in the human body duringmotion”, PhD thesis, Universite Catholique de Louvain, Louvain-la-Neuve, Belgium,2009.

[6] Reichenbach, T., ”A dynamic simulator for humanoid robots,” Artificial Life andRobotics, Vol. 13, no. 2, 561-565, 2009.

[7] Samin, J.C. and Fisette, P., ”Symbolic modelling of multibody systems,” Springer, TheNetherlands, 2003.

[8] Tsagarakis, N.G., Metta, G., Sandini, G., Vernon, D., Beira, R., Santos-Victor, J., Car-razzo, M.C., Becchi, F. and Caldwell, D.G., ”iCub - The Design and Realization ofan Open Humanoid Platform for Cognitive and Neuroscience Research,” InternationalJournal of Advanced Robotics, Vol. 21, no. 10, pp. 1151-1175, 2007.

[9] Kajita, S. and Kanehiro, F. and Kaneko, K. and Fujiwara, K. and Harada, K. and Yokoi,K. and Hirukawa, H., ”Biped walking pattern generation by using preview control ofzero-moment point,” IEEE International Conference on Robotics and Automation ICRA,Vol. 2, pp. 1620-1626, May, 2003.

[10] N.G. Tsagarakis, Zhibin Li, Jody A. Saglia, Darwin G. Caldwell, “The Design of theLower Body of the Compliant Humanoid Robot “cCub””, IEEE International Confer-ence on Robotics and Automation Conference (accepted), Shanghai, 2011.

[11] D.M. Gan, N.G.Tsagarakis, Jian Dai, Darwin G. Caldwell , “Joint Stiffness Tuningfor Compliant Robots: Protecting the Robot under Accidental Impacts”, 13th WorldCongress in Mechanism and Machine Science (accepted), 2011.

48

References

[12] Zhibin Li, Bram Vanderborght, N.G. Tsagarakis, Darwin Caldwell, “Fast Bipedal WalkUsing Large Strides by Modulating Hip Posture and Toe-heel Motion” , IEEE ROBIO,2010.

[13] Zhibin Li, Bram Vanderborght, N.G. Tsagarakis, Darwin Caldwell, “Human-like Walk-ing with Straightened Knees, Toe-off and Heel-strike for the Humanoid Robot iCub”,UK Automatic Control Conference (UKACC), Coventry, 7-10 September, 2010.

[14] G. Medrano-Cerda, H. Dallali, M. Brown, N. G. Tsagarakis, D. G. Caldwell, “Mod-elling and Simulation of the Locomotion of Humanoid Robots”, UK Automatic ControlConference (UKACC), Coventry, 7-10 September, 2010.

[15] Zhibin Li, Nikos G. Tsagarakis, and Darwin G. Caldwell, “Limit Cycle Walking forCompliant Humanoid Robot ’cCub’ ”, IROS 2011 (submitted), 2011.

49

publish compliance extension to icub simulator · could also be used for different robots including...

Documents