From b90048dc2eb5f8ed5e3ea5c2242e86ced221eaa0 Mon Sep 17 00:00:00 2001 From: Gabriele Date: Sun, 5 Feb 2017 21:55:25 +0100 Subject: [PATCH] added elastic joint control for icub --- .../tools/elasticJoints/addElasticJoints.m | 46 +++-- .../init/runController.m | 5 + .../initializeTorqueBalancing.m | 6 +- .../src/stackOfTaskController.m | 10 +- controllers/torqueBalancing_JE/config/gains.m | 74 ++++++++ .../config/trajectoryGenerator.m | 81 +++++++++ .../init/initForwardDynamics.m | 92 ++++++++++ .../torqueBalancing_JE/init/initVisualizer.m | 162 +++++++++++++++++ .../torqueBalancing_JE/init/runController.m | 79 ++++++++ .../initializeTorqueBalancing.m | 140 ++++++++++++++ .../torqueBalancing_JE/src/forwardDynamics.m | 102 +++++++++++ .../src/stackOfTaskController.m | 172 ++++++++++++++++++ 12 files changed, 954 insertions(+), 15 deletions(-) create mode 100644 controllers/torqueBalancing_JE/config/gains.m create mode 100644 controllers/torqueBalancing_JE/config/trajectoryGenerator.m create mode 100644 controllers/torqueBalancing_JE/init/initForwardDynamics.m create mode 100644 controllers/torqueBalancing_JE/init/initVisualizer.m create mode 100644 controllers/torqueBalancing_JE/init/runController.m create mode 100644 controllers/torqueBalancing_JE/initializeTorqueBalancing.m create mode 100644 controllers/torqueBalancing_JE/src/forwardDynamics.m create mode 100644 controllers/torqueBalancing_JE/src/stackOfTaskController.m diff --git a/controllers/tools/elasticJoints/addElasticJoints.m b/controllers/tools/elasticJoints/addElasticJoints.m index 7fca9bd..8804057 100644 --- a/controllers/tools/elasticJoints/addElasticJoints.m +++ b/controllers/tools/elasticJoints/addElasticJoints.m @@ -13,20 +13,44 @@ % ------------Initialization---------------- %% Configure parameters ndof = CONFIG.ndof; -% reduction ratio -r = 100; +robot_name = CONFIG.robot_name; -%% Motor dynamics -% motor inertia -ELASTICITY.B = 5.480e-5*(r^2)*eye(ndof); -% stiffness -ELASTICITY.KS = 1000*eye(ndof); +if strcmp(robot_name,'bigman') == 1 + + % reduction ratio + r = 100; -% damping -ELASTICITY.KD = 1000*eye(ndof); + %% Motor dynamics + % motor inertia + ELASTICITY.B = 5.480e-5*(r^2)*eye(ndof); -% contorl gains -ELASTICITY.KD_gain = 1000*eye(ndof); + % stiffness + ELASTICITY.KS = 0.005*(r^2)*eye(ndof); + + % damping + ELASTICITY.KD = 0.0005*(r^2)*eye(ndof); + + % control gains + ELASTICITY.KD_gain = 100*eye(ndof); + +elseif strcmp(robot_name,'icubGazeboSim') == 1 + + % reduction ratio + r = 100; + + %% Motor dynamics + % motor inertia + ELASTICITY.B = 1e-5*(r^2)*eye(ndof); + + % stiffness + ELASTICITY.KS = 0.001*(r^2)*eye(ndof); + + % damping + ELASTICITY.KD = 0.0001*(r^2)*eye(ndof); + + % control gains + ELASTICITY.KD_gain = 10*eye(ndof); +end end diff --git a/controllers/torqueBalancingWalkman_JE/init/runController.m b/controllers/torqueBalancingWalkman_JE/init/runController.m index 7ccc058..64a44f1 100644 --- a/controllers/torqueBalancingWalkman_JE/init/runController.m +++ b/controllers/torqueBalancingWalkman_JE/init/runController.m @@ -53,6 +53,11 @@ % backstepping on the motor dynamics: desired MOTOR torques (always called tau) [controlParam.tau_m,controlParam.dtheta_ref] = motorController(dtheta,theta,ELASTICITY,STATE,controlParam); +if CONFIG.assume_rigid_joints == 1 + + controlParam.tau_m = controlParam.tauModel + controlParam.Sigma*controlParam.fcDes; +end + %% Feet pose correction % this will avoid numerical errors during the forward dynamics integration if feet_on_ground(1) == 1 && feet_on_ground(2) == 0 diff --git a/controllers/torqueBalancingWalkman_JE/initializeTorqueBalancing.m b/controllers/torqueBalancingWalkman_JE/initializeTorqueBalancing.m index 66dec81..e89f43e 100644 --- a/controllers/torqueBalancingWalkman_JE/initializeTorqueBalancing.m +++ b/controllers/torqueBalancingWalkman_JE/initializeTorqueBalancing.m @@ -26,6 +26,8 @@ CONFIG.demo_movements = 1; %either 0 or 1 CONFIG.feet_on_ground = [1,1]; %either 0 or 1; [left foot,right foot] CONFIG.use_QPsolver = 1; %either 0 or 1 +CONFIG.robot_name = 'bigman'; +CONFIG.assume_rigid_joints = 1; %either 0 or 1 %% Visualization setup % robot simulator @@ -37,7 +39,7 @@ %% Integration time [s] CONFIG.tStart = 0; -CONFIG.tEnd = 10; +CONFIG.tEnd = 2.5; CONFIG.sim_step = 0.01; %% %%%%%%%%%%%%%%%%%%%%%%%%%% ADVANCED SETUP %%%%%%%%%%%%%%%%%%%%%%%%%%% %% @@ -79,7 +81,7 @@ CONFIG.figureCont = 1; %% Initialize the robot model -wbm_modelInitialize('bigman'); +wbm_modelInitialize(CONFIG.robot_name); CONFIG.ndof = 25; %% Initial joints position [deg] diff --git a/controllers/torqueBalancingWalkman_JE/src/stackOfTaskController.m b/controllers/torqueBalancingWalkman_JE/src/stackOfTaskController.m index 22c8cd0..711025b 100644 --- a/controllers/torqueBalancingWalkman_JE/src/stackOfTaskController.m +++ b/controllers/torqueBalancingWalkman_JE/src/stackOfTaskController.m @@ -136,8 +136,14 @@ % reference for motor variables and contact forces nullspace controlParam.ddtheta_ref = zeros(ndof,1); -tauModel = pinvLambda*(JcMinv*h - dJc_nu) + ELASTICITY.KS*(qj-theta)+ELASTICITY.KD*dqj + NullLambda*(h(7:end) -Mbj'/Mb*h(1:6)... - + Mbar*ddqjRef - impedances*posturalCorr*qjTilde - dampings*posturalCorr*dqjTilde); + +if CONFIG.assume_rigid_joints == 1 + tauModel = pinvLambda*(JcMinv*h - dJc_nu) + NullLambda*(h(7:end) -Mbj'/Mb*h(1:6)... + + Mbar*ddqjRef - impedances*posturalCorr*qjTilde - dampings*posturalCorr*dqjTilde); +else + tauModel = pinvLambda*(JcMinv*h - dJc_nu) + ELASTICITY.KS*(qj-theta)+ELASTICITY.KD*dqj + NullLambda*(h(7:end) -Mbj'/Mb*h(1:6)... + + Mbar*ddqjRef - impedances*posturalCorr*qjTilde - dampings*posturalCorr*dqjTilde); +end %% Desired contact forces computation fcHDot = pinvA*(HDotDes - f_grav); diff --git a/controllers/torqueBalancing_JE/config/gains.m b/controllers/torqueBalancing_JE/config/gains.m new file mode 100644 index 0000000..255d546 --- /dev/null +++ b/controllers/torqueBalancing_JE/config/gains.m @@ -0,0 +1,74 @@ +function gainsInit = gains(CONFIG) +%GAINS generates initial control gains for both the momentum task +%(primary task in SoT controller) and the postural task. +% +% gains = GAINS(CONFIG) takes as an input the structure CONFIG, which +% contains all the robot configuration parameters. The output is the structure +% gainsInit, which contains the initial gains matrices. +% +% Author : Gabriele Nava (gabriele.nava@iit.it) +% Genova, May 2016 +% + +% ------------Initialization---------------- +% Config parameters +ndof = CONFIG.ndof; + +%% Gains for two feet on ground +if sum(CONFIG.feet_on_ground) == 2 + + % CoM and angular momentum gains + gainsPCoM = diag([40 45 40]); + gainsDCoM = 2*sqrt(gainsPCoM); + gainsPAngMom = diag([1 5 1]); + gainsDAngMom = 2*sqrt(gainsPAngMom); + + % impedances acting in the null space of the desired contact forces + impTorso = [ 20 30 20]; + impArms = [ 10 10 10 5 5]; + impLeftLeg = [ 35 40 10 30 5 10]; + impRightLeg = [ 35 40 10 30 5 10]; +end + +%% Parameters for one foot on ground +if sum(CONFIG.feet_on_ground) == 1 + + % CoM and angular momentum gains + gainsPCoM = diag([30 35 30]); + gainsDCoM = 2*sqrt(gainsPCoM); + gainsPAngMom = diag([2.5 5 2.5]); + gainsDAngMom = 2*sqrt(gainsPAngMom); + + % impedances acting in the null space of the desired contact forces + impTorso = [ 10 15 10]; + impArms = [ 5 5 5 2.5 2.5]; + + if CONFIG.feet_on_ground(1) == 1 + + impLeftLeg = [ 15 15 15 15 5 5]; + impRightLeg = [ 10 10 15 15 5 5]; + else + impLeftLeg = [ 10 10 15 15 5 5]; + impRightLeg = [ 15 15 15 15 5 5]; + end +end + +%% Definition of the impedances and dampings vectors +gainsInit.impedances = [impTorso,impArms,impArms,impLeftLeg,impRightLeg]; +gainsInit.dampings = 2*sqrt(gainsInit.impedances); + +if (size(gainsInit.impedances,2) ~= ndof) + + error('Dimension mismatch between ndof and dimension of the variable impedences. Check these variables in the file gains.m'); +end + +%% Momentum and postural gains +gainsInit.impedances = diag(gainsInit.impedances); +gainsInit.dampings = diag(gainsInit.dampings); +gainsInit.momentumGains = [gainsDCoM zeros(3); zeros(3) gainsDAngMom]; +gainsInit.intMomentumGains = [gainsPCoM zeros(3); zeros(3) gainsPAngMom]; + +% gains for feet correction to avoid numerical intrgration errors +gainsInit.corrPosFeet = 100; + +end diff --git a/controllers/torqueBalancing_JE/config/trajectoryGenerator.m b/controllers/torqueBalancing_JE/config/trajectoryGenerator.m new file mode 100644 index 0000000..9439024 --- /dev/null +++ b/controllers/torqueBalancing_JE/config/trajectoryGenerator.m @@ -0,0 +1,81 @@ +function desired_x_dx_ddx_CoM = trajectoryGenerator(xCoMInit,t,CONFIG) +%TRAJECTORYGENERATOR generates a desired CoM trajectory. The default trajectory +% is a sinusoid in the Y direction. +% +% desired_x_dx_ddx_CoM = TRAJECTORYGENERATOR(xCoMInit,t,CONFIG) takes as an +% input the initial CoM position, xCoMInit, the current time t and the structure +% CONFIG which contains the user-defined parameters. +% The output desired_x_dx_ddx_CoM is a matrix [3x3] which contains the CoM +% reference acceleration, velocity and position. +% +% Author : Gabriele Nava (gabriele.nava@iit.it) +% Genova, May 2016 +% + +% ------------Initialization---------------- +% Config parameters +feet_on_ground = CONFIG.feet_on_ground; +demo_movements = CONFIG.demo_movements; + +% Initial parameters +directionOfOscillation = [0;0;0]; +referenceParams = [0.0 0.0]; %referenceParams(1) = amplitude of ascillations in meters + %referenceParams(2) = frequency of ascillations in Hertz + +% NB: IF THE INTEGRATION IS PERFORMED USING ODE15S THIS PARAMETER SHOULD REMAIN ZERO +noOscillationTime = 0; % If params.demo_movements = 1, the variable noOscillationTime is the time, in seconds, + % that the robot waits before starting the left-and-right + +%% Trajectory definition +if demo_movements == 1 + + if sum(feet_on_ground) == 2 + + directionOfOscillation = [0;1;0]; + referenceParams = [0.035 0.35]; + else + + directionOfOscillation = [0;1;0]; + referenceParams = [0.015 0.15]; + end +end + +%% Trajectory generation +frequency = referenceParams(2); + +if size(t,1)==1 && size(t,2)==1 + + if t >= noOscillationTime + + amplitude = referenceParams(1); + else + amplitude = 0; + end + + xCoMDes = xCoMInit + amplitude*sin(2*pi*frequency*t)*directionOfOscillation; + dxCoMDes = amplitude*2*pi*frequency*cos(2*pi*frequency*t)*directionOfOscillation; + ddxCoMDes = - amplitude*(2*pi*frequency)^2*sin(2*pi*frequency*t)*directionOfOscillation; + + desired_x_dx_ddx_CoM = [xCoMDes dxCoMDes ddxCoMDes]; +else + + % in case t is a vector + desired_x_dx_ddx_CoM = zeros(3,3,length(t)); + + for k = 1:length(t) + if t(k) >= noOscillationTime + + amplitude = referenceParams(1); + else + amplitude = 0; + end + + xCoMDes = xCoMInit + amplitude*sin(2*pi*frequency*t(k))*directionOfOscillation; + dxCoMDes = amplitude*2*pi*frequency*cos(2*pi*frequency*t(k))*directionOfOscillation; + ddxCoMDes = - amplitude*(2*pi*frequency)^2*sin(2*pi*frequency*t(k))*directionOfOscillation; + + desired_x_dx_ddx_CoM(:,:,k) = [xCoMDes dxCoMDes ddxCoMDes]; + end +end + + diff --git a/controllers/torqueBalancing_JE/init/initForwardDynamics.m b/controllers/torqueBalancing_JE/init/initForwardDynamics.m new file mode 100644 index 0000000..f00d31d --- /dev/null +++ b/controllers/torqueBalancing_JE/init/initForwardDynamics.m @@ -0,0 +1,92 @@ +function [] = initForwardDynamics(CONFIG) +%INITFORWARDDYNAMICS configures the forward dynamics integration by setting +% state initial conditions, control gains, etc. +% +% [] = INITFORWARDDYNAMICS(CONFIG) takes as input the structure CONFIG +% containing the initial user-defined parameters. It has no output. The +% forward dynamics integration will be performed following the options +% the user specified in the initialization script. +% +% Author : Gabriele Nava (gabriele.nava@iit.it) +% Genova, May 2016 +% + +% ------------Initialization---------------- +%% Setup the configuration and state parameters +feet_on_ground = CONFIG.feet_on_ground; +ndof = CONFIG.ndof; +qjInit = CONFIG.qjInit; +dqjInit = zeros(ndof,1); +dx_bInit = zeros(3,1); +w_omega_bInit = zeros(3,1); + +% initialize motor state +thetaInit = qjInit; +dthetaInit = dqjInit; +chi_motorInit = [thetaInit;dthetaInit]; + +%% Contact constraints definition +if sum(feet_on_ground) == 2 + + CONFIG.constraintLinkNames = {'l_sole','r_sole'}; + +elseif feet_on_ground(1) == 1 && feet_on_ground(2) == 0 + + CONFIG.constraintLinkNames = {'l_sole'}; + +elseif feet_on_ground(1) == 0 && feet_on_ground(2) == 1 + + CONFIG.constraintLinkNames = {'r_sole'}; +end + +CONFIG.numConstraints = length(CONFIG.constraintLinkNames); + +%% Configure the model using initial conditions +wbm_updateState(qjInit,dqjInit,[dx_bInit;w_omega_bInit]); + +% fixing the world reference frame w.r.t. the foot on ground position +if feet_on_ground(1) == 1 + + [x_bInit,w_R_bInit] = wbm_getWorldFrameFromFixLnk('l_sole',qjInit); +else + [x_bInit,w_R_bInit] = wbm_getWorldFrameFromFixLnk('r_sole',qjInit); +end + +wbm_setWorldFrame(w_R_bInit,x_bInit,[0 0 -9.81]') + +% initial state (floating base + joints) +[basePoseInit,~,~,~] = wbm_getState(); +chi_robotInit = [basePoseInit; qjInit; dx_bInit; w_omega_bInit; dqjInit]; + +%% Initial state that consider also joints elasticity +chiInit = [chi_robotInit; chi_motorInit]; + +%% Initial gains +% the initial gains are defined before the numerical integration +CONFIG.gainsInit = gains(CONFIG); + +%% Initial dynamics and forward kinematics +% initial state +CONFIG.initState = robotState(chi_robotInit,CONFIG); +% initial dynamics +CONFIG.initDynamics = robotDynamics(CONFIG.initState,CONFIG); +% initial forward kinematics +CONFIG.initForKinematics = robotForKinematics(CONFIG.initState,CONFIG.initDynamics); + +%% Forward dynamics integration +CONFIG.wait = waitbar(0,'Forward dynamics integration...'); +forwardDynFunc = @(t,chi)forwardDynamics(t,chi,CONFIG); + +% either fixed step integrator or ODE15s +if CONFIG.integrateWithFixedStep == 1 + [t,chi] = euleroForward(forwardDynFunc,chiInit,CONFIG.tEnd,CONFIG.tStart,CONFIG.sim_step); +else + [t,chi] = ode15s(forwardDynFunc,CONFIG.tStart:CONFIG.sim_step:CONFIG.tEnd,chiInit,CONFIG.options); +end + +delete(CONFIG.wait) + +%% Visualize integration results and robot simulator +CONFIG.figureCont = initVisualizer(t,chi,CONFIG); + +end diff --git a/controllers/torqueBalancing_JE/init/initVisualizer.m b/controllers/torqueBalancing_JE/init/initVisualizer.m new file mode 100644 index 0000000..a0bd6d1 --- /dev/null +++ b/controllers/torqueBalancing_JE/init/initVisualizer.m @@ -0,0 +1,162 @@ +function figureCont = initVisualizer(t,chi,CONFIG) +%INITVISUALIZER initializes the visualization of forward dynamics +% integration results. +% +% INITVISUALIZER visualizes results of the forward dynamics +% integration (e.g. the robot state, contact forces, control torques...) +% and initialize robot simulator. +% +% figureCont = INITVISUALIZER(t,chi,CONFIG) takes as input the integration +% time t, the robot state chi and the robot configuration. The output is a +% counter for the automatic correction of figures numbers in case a new +% figure is added. +% +% Author : Gabriele Nava (gabriele.nava@iit.it) +% Genova, May 2016 +% + +% ------------Initialization---------------- +%% Configuration parameters +ndof = CONFIG.ndof; +initState = CONFIG.initState; +figureCont = CONFIG.figureCont; + +%% Robot simulator +if CONFIG.visualize_robot_simulator == 1 + % list of joints used in the visualizer + CONFIG.modelName = 'iCub'; + CONFIG.setPos = [1,0,0.5]; + CONFIG.setCamera = [0.4,0,0.5]; + CONFIG.mdlLdr = iDynTree.ModelLoader(); + consideredJoints = iDynTree.StringVector(); + + consideredJoints.push_back('torso_pitch'); + consideredJoints.push_back('torso_roll'); + consideredJoints.push_back('torso_yaw'); + consideredJoints.push_back('l_shoulder_pitch'); + consideredJoints.push_back('l_shoulder_roll'); + consideredJoints.push_back('l_shoulder_yaw'); + consideredJoints.push_back('l_elbow'); + consideredJoints.push_back('l_wrist_prosup'); + consideredJoints.push_back('r_shoulder_pitch'); + consideredJoints.push_back('r_shoulder_roll'); + consideredJoints.push_back('r_shoulder_yaw'); + consideredJoints.push_back('r_elbow'); + consideredJoints.push_back('r_wrist_prosup'); + consideredJoints.push_back('l_hip_pitch'); + consideredJoints.push_back('l_hip_roll'); + consideredJoints.push_back('l_hip_yaw'); + consideredJoints.push_back('l_knee'); + consideredJoints.push_back('l_ankle_pitch'); + consideredJoints.push_back('l_ankle_roll'); + consideredJoints.push_back('r_hip_pitch'); + consideredJoints.push_back('r_hip_roll'); + consideredJoints.push_back('r_hip_yaw'); + consideredJoints.push_back('r_knee'); + consideredJoints.push_back('r_ankle_pitch'); + consideredJoints.push_back('r_ankle_roll'); + + % load the model from .urdf + CONFIG.mdlLdr.loadReducedModelFromFile('../models/icub/model.urdf',consideredJoints); + + % set lights + CONFIG.lightDir = iDynTree.Direction(); + CONFIG.lightDir.fromMatlab([-0.5 0 -0.5]/sqrt(2)); + + visualizeSimulation_iDyntree(chi,CONFIG); +end + +%% Forward dynamics results +if CONFIG.visualize_integration_results == 1 || CONFIG.visualize_joints_dynamics == 1 || CONFIG.visualize_motors_dynamics == 1 + + CONFIG.wait = waitbar(0,'Generating the results...'); + set(0,'DefaultFigureWindowStyle','Docked'); + + % joints initialization + qj = zeros(ndof,length(t)); + qjInit = zeros(ndof,length(t)); + qjRef = zeros(ndof,length(t)); + + % motors variables + theta = zeros(ndof,length(t)); + dtheta = zeros(ndof,length(t)); + dtheta_ref = zeros(ndof,length(t)); + + % contact forces and torques initialization + fc = zeros(6*CONFIG.numConstraints,length(t)); + f0 = zeros(6*CONFIG.numConstraints,length(t)); + tau_m = zeros(ndof,length(t)); + tau_norm = zeros(length(t),1); + + % forward kinematics initialization + xCoM = zeros(3,length(t)); + poseFeet = zeros(12,length(t)); + CoP = zeros(4,length(t)); + H = zeros(6,length(t)); + HRef = zeros(6,length(t)); + + % generate the vectors from forward dynamics + for time = 1:length(t) + + [~,visual] = forwardDynamics(t(time), chi(time,:)', CONFIG); + + % joints dynamics + qj(:,time) = visual.qj; + qjInit(:,time) = initState.qj; + qjRef(:,time) = visual.jointRef.qjRef; + + % motor dynamics + theta(:,time) = visual.theta; + dtheta(:,time) = visual.dtheta; + dtheta_ref(:,time) = visual.dtheta_ref; + + %% Other parameters + % contact forces and torques + fc(:,time) = visual.fc; + f0(:,time) = visual.f0; + tau_m(:,time) = visual.tau_m; + tau_norm(time) = norm(visual.tau_m); + + % forward kinematics + xCoM(:,time) = visual.xCoM; + poseFeet(:,time) = visual.poseFeet; + H(:,time) = visual.H; + HRef(:,time) = visual.HRef; + + % centers of pressure at feet + CoP(1,time) = -visual.fc(5)/visual.fc(3); + CoP(2,time) = visual.fc(4)/visual.fc(3); + + if CONFIG.numConstraints == 2 + + CoP(3,time) = -visual.fc(11)/visual.fc(9); + CoP(4,time) = visual.fc(10)/visual.fc(9); + end + + end + + delete(CONFIG.wait) + HErr = H-HRef; + + %% Basic visualization (forward dynamics integration results) + if CONFIG.visualize_integration_results == 1 + + CONFIG.figureCont = visualizeForwardDyn(t,CONFIG,xCoM,poseFeet,fc,f0,tau_norm,CoP,HErr); + end + + %% Joints positions and position error + if CONFIG.visualize_joints_dynamics == 1 + + CONFIG.figureCont = visualizeJointDynamics(t,CONFIG,qj,qjRef); + end + + %% Motors dynamics + if CONFIG.visualize_motors_dynamics == 1 + + CONFIG.figureCont = visualizeMotorsDynamics(t,CONFIG,theta,dtheta_ref,dtheta); + end + + figureCont = CONFIG.figureCont; + set(0,'DefaultFigureWindowStyle','Normal'); + +end diff --git a/controllers/torqueBalancing_JE/init/runController.m b/controllers/torqueBalancing_JE/init/runController.m new file mode 100644 index 0000000..64a44f1 --- /dev/null +++ b/controllers/torqueBalancing_JE/init/runController.m @@ -0,0 +1,79 @@ +function controlParam = runController(gain,trajectory,DYNAMICS,FORKINEMATICS,CONFIG,STATE,dtheta,theta,ELASTICITY) +%RUNCONTROLLER initializes balancing controllers. Default controller uses +% "stack of tasks" approach. +% +% controlParam = RUNCONTROLLER(gains,trajectory,DYNAMICS,FORKINEMATICS, +% CONFIG,STATE) takes as input control gains, joint reference trajectory, +% and the robot dynamics, forward kinematicsn, state and user-defined +% configuration. The output is structure controlparam which contains the +% control torques tau, the contact forces fc, parameters for visualizing the +% results, etc. +% +% Author : Gabriele Nava (gabriele.nava@iit.it) +% Genova, May 2016 + +% ------------Initialization---------------- +%% Feet correction gains +KpFeet = gain.corrPosFeet; +KdFeet = 2*sqrt(KpFeet); + +%% Configuration parameters +ndof = CONFIG.ndof; +use_QPsolver = CONFIG.use_QPsolver; +feet_on_ground = CONFIG.feet_on_ground; +initForKinematics = CONFIG.initForKinematics; +S = [zeros(6,ndof); + eye(ndof,ndof)]; + +%% Robot dynamics +dJc_nu = DYNAMICS.dJc_nu; +M = DYNAMICS.M; +Jc = DYNAMICS.Jc; +h = DYNAMICS.h; +JcMinv = Jc/M; +JcMinvS = JcMinv*S; + +%% Robot forward Kinematics +v_feet = FORKINEMATICS.v_feet; +TL = FORKINEMATICS.TL; +TR = FORKINEMATICS.TR; +poseRFoot_ang = FORKINEMATICS.poseRFoot_ang; +poseLFoot_ang = FORKINEMATICS.poseLFoot_ang; +deltaPoseRFoot = TR*(poseRFoot_ang-initForKinematics.poseRFoot_ang); +deltaPoseLFoot = TL*(poseLFoot_ang-initForKinematics.poseLFoot_ang); + +%% BALANCING CONTROLLER: the current version uses a stack of task approach +controlParam = stackOfTaskController(CONFIG,gain,trajectory,DYNAMICS,FORKINEMATICS,STATE,theta,ELASTICITY); + +if use_QPsolver == 1 + % quadratic programming solver for the nullspace of contact forces + controlParam.fcDes = QPSolver(controlParam,CONFIG,FORKINEMATICS); +end + +% backstepping on the motor dynamics: desired MOTOR torques (always called tau) +[controlParam.tau_m,controlParam.dtheta_ref] = motorController(dtheta,theta,ELASTICITY,STATE,controlParam); + +if CONFIG.assume_rigid_joints == 1 + + controlParam.tau_m = controlParam.tauModel + controlParam.Sigma*controlParam.fcDes; +end + +%% Feet pose correction +% this will avoid numerical errors during the forward dynamics integration +if feet_on_ground(1) == 1 && feet_on_ground(2) == 0 + + deltaPoseFeet = deltaPoseLFoot; + +elseif feet_on_ground(1) == 0 && feet_on_ground(2) == 1 + + deltaPoseFeet = deltaPoseRFoot; + +elseif feet_on_ground(1) == 1 && feet_on_ground(2) == 1 + + deltaPoseFeet = [deltaPoseLFoot;deltaPoseRFoot]; +end + +%% Contact forces computation +controlParam.fc = (JcMinv*transpose(Jc))\(JcMinv*h -JcMinvS*(ELASTICITY.KS*(theta-STATE.qj)+ELASTICITY.KD*(dtheta-STATE.dqj)) -dJc_nu -KdFeet.*v_feet-KpFeet.*deltaPoseFeet); + +end diff --git a/controllers/torqueBalancing_JE/initializeTorqueBalancing.m b/controllers/torqueBalancing_JE/initializeTorqueBalancing.m new file mode 100644 index 0000000..853fb00 --- /dev/null +++ b/controllers/torqueBalancing_JE/initializeTorqueBalancing.m @@ -0,0 +1,140 @@ +%% INITIALIZETORQUEBALANCING +% +% This is the initialization script for torque balancing simulations of +% floating base robots using Matlab. +% +% Forward dynamics integration is available for the robot balancing on one +% foot or two feet (6 or 12 contact constraints, respectively). The controller +% ensures stability properties of the system around any set point in case of +% k=1 (see [Nava et al, IROS 2016]. +% Contact forces are evaluated though QP solver to ensure unilateral constraints, +% and to constrain them inside the friction cones. +% +% This version has been modified in order to consider joints elasticity in +% the model. +% +% Author : Gabriele Nava (gabriele.nava@iit.it) +% Genova, May 2016 +% + +% ------------Initialization---------------- +clear all +close all +clc +%% %%%%%%%%%%%%%%%%%%%%%%%%%%% BASIC SETUP %%%%%%%%%%%%%%%%%%%%%%%%%%%%% %% +%% Configure the simulation +CONFIG.demo_movements = 1; %either 0 or 1 +CONFIG.feet_on_ground = [1,1]; %either 0 or 1; [left foot,right foot] +CONFIG.use_QPsolver = 1; %either 0 or 1 +CONFIG.robot_name = 'icubGazeboSim'; +CONFIG.assume_rigid_joints = 0; %either 0 or 1 + +%% Visualization setup +% robot simulator +CONFIG.visualize_robot_simulator = 1; %either 0 or 1 +% forward dynamics integration results +CONFIG.visualize_integration_results = 0; %either 0 or 1 +CONFIG.visualize_joints_dynamics = 0; %either 0 or 1 +CONFIG.visualize_motors_dynamics = 1; + +%% Integration time [s] +CONFIG.tStart = 0; +CONFIG.tEnd = 2.5; +CONFIG.sim_step = 0.01; + +%% %%%%%%%%%%%%%%%%%%%%%%%%%% ADVANCED SETUP %%%%%%%%%%%%%%%%%%%%%%%%%%% %% +% ONLY FOR DEVELOPERS +% tolerances for pseudoinverse and QP +CONFIG.pinv_tol = 1e-8; +CONFIG.pinv_damp = 5e-6; +CONFIG.reg_HessianQP = 1e-3; + +%% Forward dynamics integration setup +% CONFIG.integrateWithFixedStep will use a Euler forward integrator instead +% of ODE15s to integrate the forward dynamics. It may be useful for debug. +CONFIG.integrateWithFixedStep = 0; %either 0 or 1 + +% The fixed step integration needs a desingularization of system mass matrix +% in order to converge to a solution +if CONFIG.integrateWithFixedStep == 1 + + CONFIG.massCorr = 0.05; +else + CONFIG.massCorr = 0; +end + +% Integration options. If the intrgration is slow, try to modify these +% options. +if CONFIG.demo_movements == 0 + CONFIG.options = odeset('RelTol',1e-3,'AbsTol',1e-3); +else + CONFIG.options = odeset('RelTol',1e-6,'AbsTol',1e-6); +end + +%% Visualization setup +% this script modifies the default MATLAB options for figures and graphics. +% This will result in a better visualization of the plots. +plot_set + +% this is the figure counter. It is used to automatically adapt the figure +% number in case new figures are added +CONFIG.figureCont = 1; + +%% Initialize the robot model +wbm_modelInitialize(CONFIG.robot_name); +CONFIG.ndof = 25; + +%% Initial joints position [deg] +leftArmInit = [ -20 30 0 45 0]'; +rightArmInit = [ -20 30 0 45 0]'; +torsoInit = [ -10 0 0]'; + +if sum(CONFIG.feet_on_ground) == 2 + + % initial conditions for balancing on two feet + leftLegInit = [ 25.5 0 0 -18.5 -5.5 0]'; + rightLegInit = [ 25.5 0 0 -18.5 -5.5 0]'; + +elseif CONFIG.feet_on_ground(1) == 1 && CONFIG.feet_on_ground(2) == 0 + + % initial conditions for the robot standing on the left foot + leftLegInit = [ 25.5 15 0 -18.5 -5.5 0]'; + rightLegInit = [ 25.5 5 0 -40 -5.5 0]'; + +elseif CONFIG.feet_on_ground(1) == 0 && CONFIG.feet_on_ground(2) == 1 + + % initial conditions for the robot standing on the right foot + leftLegInit = [ 25.5 5 0 -40 -5.5 0]'; + rightLegInit = [ 25.5 15 0 -18.5 -5.5 0]'; +end + +% feet size +CONFIG.footSize = [-0.07 0.07; % xMin, xMax + -0.03 0.03]; % yMin, yMax +% joints configuration [rad] +CONFIG.qjInit = [torsoInit;leftArmInit;rightArmInit;leftLegInit;rightLegInit]*(pi/180); + +%% Paths definition and initialize the forward dynamics integration +% add the required paths. This procedure will make the paths consistent for +% any starting folder. +codyco_root = getenv('CODYCO_SUPERBUILD_ROOT'); +utility_root = [codyco_root, filesep, '/main/mexWholeBodyModel/controllers/tools']; +robot_root = [utility_root, filesep, '/robotFunctions']; +plots_root = [utility_root, filesep, '/visualization']; +EJoints_root = [utility_root, filesep, '/elasticJoints']; +src_root = [codyco_root, filesep, '/main/mexWholeBodyModel/controllers/torqueBalancing_JE/src']; +config_root = [codyco_root, filesep, '/main/mexWholeBodyModel/controllers/torqueBalancing_JE/config']; +init_root = [codyco_root, filesep, '/main/mexWholeBodyModel/controllers/torqueBalancing_JE/init']; + +% add the paths +addpath(utility_root); +addpath(robot_root); +addpath(plots_root); +addpath(src_root); +addpath(config_root); +addpath(init_root); +addpath(EJoints_root); + +%% INITIALIZATION +% initialize the forward dynamics +initForwardDynamics(CONFIG); diff --git a/controllers/torqueBalancing_JE/src/forwardDynamics.m b/controllers/torqueBalancing_JE/src/forwardDynamics.m new file mode 100644 index 0000000..1d8cfa8 --- /dev/null +++ b/controllers/torqueBalancing_JE/src/forwardDynamics.m @@ -0,0 +1,102 @@ +function [dchi,visualization] = forwardDynamics(t,chi,CONFIG) +%FORWARDDYNAMICS computes the forward dynamics of floating base robots +% given initial conditions, contact forces and torques. +% +% [dchi,visualization] = FORWARDDYNAMICS(t,chi,CONFIG) takes as input the +% current time step, t; the state, chi [13+2*ndof x 1]; the structure +% CONFIG which contains initial conditions and user-defined parameters. +% The output are the vector to be integrated, dchi [13+2*ndof x1] and the +% structure visualization which is used for plotting results in the visualizer. +% +% Author : Gabriele Nava (gabriele.nava@iit.it) +% Genova, May 2016 + +% ------------Initialization---------------- +import WBM.utilities.dquat; +waitbar(t/CONFIG.tEnd,CONFIG.wait) + +%% Robot Configuration +ndof = CONFIG.ndof; +chi_robot = chi(1:(13+2*ndof)); + +% motor configuration +theta = chi(14+2*ndof:13+3*ndof); +dtheta = chi(14+3*ndof:end); +ELASTICITY = addElasticJoints(CONFIG); + +gain = CONFIG.gainsInit; +qjInit = CONFIG.qjInit; + +% CoM initial position +CONFIG.xCoMRef = CONFIG.initForKinematics.xCoM; + +%% Robot State +STATE = robotState(chi_robot,CONFIG); +w_R_b = STATE.w_R_b; +w_omega_b = STATE.w_omega_b; +qt_b = STATE.qt_b; +dx_b = STATE.dx_b; +dqj = STATE.dqj; +qj = STATE.qj; +x_b = STATE.x_b; + +%% Set the robot state (for wbm functions in 'optimized mode') +wbm_setWorldFrame(w_R_b,x_b,[0 0 -9.81]') +wbm_updateState(qj,dqj,[dx_b;w_omega_b]); + +%% Robot Dynamics +DYNAMICS = robotDynamics(STATE,CONFIG); +Jc = DYNAMICS.Jc; +M = DYNAMICS.M; +h = DYNAMICS.h; +H = DYNAMICS.H; +m = M(1,1); + +%% Robot Forward kinematics +FORKINEMATICS = robotForKinematics(STATE,DYNAMICS); +poseRFoot_ang = FORKINEMATICS.poseRFoot_ang; +poseLFoot_ang = FORKINEMATICS.poseLFoot_ang; +xCoM = FORKINEMATICS.xCoM; + +%% Joint limits check +% jointLimitsCheck(qj,t); + +%% CoM and joints trajectory generator +trajectory.jointReferences.ddqjRef = zeros(ndof,1); +trajectory.jointReferences.dqjRef = zeros(ndof,1); +trajectory.jointReferences.qjRef = qjInit; +trajectory.desired_x_dx_ddx_CoM = trajectoryGenerator(CONFIG.xCoMRef,t,CONFIG); + +%% Torque balancing controller +controlParam = runController(gain,trajectory,DYNAMICS,FORKINEMATICS,CONFIG,STATE,dtheta,theta,ELASTICITY); +tau_m = controlParam.tau_m; +fc = controlParam.fc; + +%% State derivative (dchi) computation +b_omega_w = transpose(w_R_b)*w_omega_b; +dq_b = dquat(qt_b,b_omega_w); +nu = [dx_b;dq_b;dqj]; +dnu = M\(Jc'*fc + [zeros(6,1);(ELASTICITY.KS*(theta-qj)+ELASTICITY.KD*(dtheta-dqj))]-h); + +% state derivative +dchi_robot = [nu;dnu]; +% motor derivative +ddtheta = ELASTICITY.B\(tau_m+ELASTICITY.KS*(qj-theta)+ELASTICITY.KD*(dqj-dtheta)); +% total state derivative +dchi = [dchi_robot;dtheta;ddtheta]; + +%% Parameters for visualization +visualization.theta = theta; +visualization.dtheta = dtheta; +visualization.dtheta_ref = controlParam.dtheta_ref; +visualization.qj = qj; +visualization.jointRef = trajectory.jointReferences; +visualization.xCoM = xCoM; +visualization.poseFeet = [poseLFoot_ang;poseRFoot_ang]; +visualization.H = H; +visualization.HRef = [m*trajectory.desired_x_dx_ddx_CoM(:,2);zeros(3,1)]; +visualization.fc = fc; +visualization.f0 = controlParam.f0; +visualization.tau_m = tau_m; + +end diff --git a/controllers/torqueBalancing_JE/src/stackOfTaskController.m b/controllers/torqueBalancing_JE/src/stackOfTaskController.m new file mode 100644 index 0000000..711025b --- /dev/null +++ b/controllers/torqueBalancing_JE/src/stackOfTaskController.m @@ -0,0 +1,172 @@ +function controlParam = stackOfTaskController(CONFIG,gain,trajectory,DYNAMICS,FORKINEMATICS,STATE,theta,ELASTICITY) +%STACKOFTASKCONTROLLER implements a momentum-based control algorithm for +% floating base robots. +% +% STACKOFTASKCONTROLLER computes the desired control torques at joints +% using a task-based approach. The first task is the achievement of a +% desired robot momentum, while the second task is a postural task. +% +% controlParam = STACKOFTASKCONTROLLER(CONFIG, gain, trajectory, +% DYNAMICS,FORKINEMATICS,STATE) takes as input the robot configuration, +% control gains, reference trajectory, and robot forward kinematics, +% dynamics and state. +% The output controlParam contains desired control torques and contact +% forces and others parameters used for visualization and QP solver. +% +% Author : Gabriele Nava (gabriele.nava@iit.it) +% Genova, May 2016 +% + +% ------------Initialization---------------- +import WBM.utilities.skewm; + +%% Config parameters +pinv_tol = CONFIG.pinv_tol; +pinv_damp = CONFIG.pinv_damp; +feet_on_ground = CONFIG.feet_on_ground; +ndof = CONFIG.ndof; + +%% Gains +impedances = gain.impedances; +dampings = gain.dampings; +momentumGains = gain.momentumGains; +intMomentumGains = gain.intMomentumGains; + +%% Dynamics +M = DYNAMICS.M; +g = DYNAMICS.g; +C_nu = DYNAMICS.C_nu; +dJc_nu = DYNAMICS.dJc_nu; +H = DYNAMICS.H; +Jc = DYNAMICS.Jc; +h = g + C_nu; +m = M(1,1); +Mb = M(1:6,1:6); +Mbj = M(1:6,7:end); +Mj = M(7:end,7:end); +Mjb = M(7:end,1:6); +Mbar = Mj - Mjb/Mb*Mbj; +% The centroidal momentum jacobian is reduced to the joint velocity. This +% is then used to compute the approximation of the angular momentum integral +JH = DYNAMICS.JH; +JG = JH(:,7:end) - JH(:,1:6)*(eye(6)/Jc(1:6,1:6))*Jc(1:6,7:end); + +%% Forward kinematics +xCoM = FORKINEMATICS.xCoM; +dxCoM = FORKINEMATICS.dxCoM; +poseRFoot_ang = FORKINEMATICS.poseRFoot_ang; +poseLFoot_ang = FORKINEMATICS.poseLFoot_ang; + +%% Robot State +qj = STATE.qj; +dqj = STATE.dqj; + +% Trajectory +x_dx_ddx_CoMDes = trajectory.desired_x_dx_ddx_CoM; +ddqjRef = trajectory.jointReferences.ddqjRef; +dqjRef = trajectory.jointReferences.dqjRef; +qjRef = trajectory.jointReferences.qjRef; +qjTilde = qj-qjRef; +dqjTilde = dqj-dqjRef; + +% General parameters +gravAcc = 9.81; +S = [zeros(6,ndof); + eye(ndof,ndof)]; +f_grav = [zeros(2,1); + -m*gravAcc; + zeros(3,1)]; + +%% Multiplier of contact wrenches at CoM +x_RFoot = poseRFoot_ang(1:3); +x_LFoot = poseLFoot_ang(1:3); +r_RF = x_RFoot - xCoM; % Application point of the contact force on the right foot w.r.t. CoM +r_LF = x_LFoot - xCoM; % Application point of the contact force on the left foot w.r.t. CoM +AL = [eye(3), zeros(3); + skewm(r_LF),eye(3)]; +AR = [eye(3), zeros(3); + skewm(r_RF),eye(3)]; + +% One foot or two feet on ground selector +if sum(feet_on_ground) == 2 + + A = [AL,AR]; + pinvA = pinv(A,pinv_tol); +else + if feet_on_ground(1) == 1 + + A = AL; + elseif feet_on_ground(2) == 1 + + A = AR; + end + pinvA = eye(6)/A; +end + +%% Desired momentum derivative +% closing the loop on angular momentum integral +deltaPhi = JG(4:end,:)*(qj-qjRef); +accDesired = [m.*x_dx_ddx_CoMDes(:,3); zeros(3,1)]; +velDesired = -momentumGains*[m.*(dxCoM-x_dx_ddx_CoMDes(:,2)); H(4:end)]; +posDesired = -intMomentumGains*[m.*(xCoM-x_dx_ddx_CoMDes(:,1)); deltaPhi]; +HDotDes = accDesired + velDesired + posDesired; + +%% Control torques equation +JcMinv = Jc/M; +Lambda = JcMinv*S; +JcMinvJct = JcMinv*transpose(Jc); +pinvLambda = pinv(Lambda,pinv_tol); + +% multiplier of contact wrenches in u0 +JBar = transpose(Jc(:,7:end)) - Mbj'/Mb*transpose(Jc(:,1:6)); + +% nullspaces +NullA = eye(6*CONFIG.numConstraints)-pinvA*A; +NullLambda = eye(ndof)-pinvLambda*Lambda; + +Sigma = -(pinvLambda*JcMinvJct + NullLambda*JBar); +SigmaNA = Sigma*NullA; + +% Postural task correction +pinvLambdaDamp = Lambda'/(Lambda*Lambda' + pinv_damp*eye(size(Lambda,1))); +NullLambdaDamp = eye(ndof)-pinvLambdaDamp*Lambda; +posturalCorr = NullLambdaDamp*Mbar; +impedances = impedances*pinv(posturalCorr,pinv_tol) + 0.01*eye(ndof); +dampings = dampings*pinv(posturalCorr,pinv_tol) + 0.01*eye(ndof); + +% reference for motor variables and contact forces nullspace +controlParam.ddtheta_ref = zeros(ndof,1); + +if CONFIG.assume_rigid_joints == 1 + tauModel = pinvLambda*(JcMinv*h - dJc_nu) + NullLambda*(h(7:end) -Mbj'/Mb*h(1:6)... + + Mbar*ddqjRef - impedances*posturalCorr*qjTilde - dampings*posturalCorr*dqjTilde); +else + tauModel = pinvLambda*(JcMinv*h - dJc_nu) + ELASTICITY.KS*(qj-theta)+ELASTICITY.KD*dqj + NullLambda*(h(7:end) -Mbj'/Mb*h(1:6)... + + Mbar*ddqjRef - impedances*posturalCorr*qjTilde - dampings*posturalCorr*dqjTilde); +end + +%% Desired contact forces computation +fcHDot = pinvA*(HDotDes - f_grav); + +% Desired f0 without Quadratic Programming +f0 = zeros(6,1); + +if sum(feet_on_ground) == 2 + + f0 = -pinv(SigmaNA,pinv_tol)*(tauModel+Sigma*fcHDot); +end + +fcDes = fcHDot + NullA*f0; + +%% Output parameters +controlParam.fcHDot = fcHDot; +controlParam.HDotDes = HDotDes; +controlParam.fcDes = fcDes; +controlParam.f_grav = f_grav; +controlParam.tauModel = tauModel; +controlParam.Sigma = Sigma; +controlParam.f0 = f0; +controlParam.NullA = NullA; +controlParam.A = A; + +end