From 3569a9c9aab8e8a0644dcd6420cc1aaf92fc9471 Mon Sep 17 00:00:00 2001 From: Nuno Guedelha Date: Mon, 10 May 2021 03:25:27 +0200 Subject: [PATCH] Update README.md Update with respect to the last PR #44 . --- README.md | 40 +++++++++++++++++++++++++--------------- 1 file changed, 25 insertions(+), 15 deletions(-) diff --git a/README.md b/README.md index 9fd161e..6b5d8cf 100644 --- a/README.md +++ b/README.md @@ -93,7 +93,7 @@ The required commands have been sequenced in a MATLAB script, `app/tools/matlabO ## :runner: How to use the simulator -1. Connect your controller to the robot block. This block takes as imput the **joints torque**, **motor inertia** and an eventual **generalized external wrench**. It outputs the robot **state**, the contact wrenches **wrench_LFoot** and **wrench_RFoot**, respectively applied to the left and right foot (sole frames), and **kinDynOut**, an output bus exposing all the computed dynamics quantities relevant for debugging or the extension of dynamics computations in external blocks (emulation of an IMU sensor, of pressure sensors on the feet, etc). +1. Connect your controller to the **RobotDynWithContacts** block. This block takes as imput the **joints torque**, **motor inertia** and an eventual **generalized external wrench**. It outputs the robot **state**, the contact wrenches **wrench_LFoot** and **wrench_RFoot**, respectively applied to the left and right foot (sole frames), and **kinDynOut**, an output bus exposing all the computed dynamics quantities relevant for debugging or the extension of dynamics computations in external blocks (emulation of an IMU sensor, of pressure sensors on the feet, etc). 2. Select the robot model by setting the environment variable `YARP_ROBOT_NAME` (e.g. `setenv('YARP_ROBOT_NAME','iCubGenova04')`). The available models are: | Model description | iCub robot (iCubGenova04) | RRBot from Gazebo | | --- | --- | --- | @@ -106,21 +106,31 @@ The required commands have been sequenced in a MATLAB script, `app/tools/matlabO **Details:** || Structure | Field name | Size/Type | Description | | --- | --- | --- | --- | --- | -|robot_config| jointOrder | {1×N_DOF cell} | List of N_DOF "controlled" joints (matches dimension of joint torques input) | -| | meshFilePrefix | '' | Prefix to be concatenated to the mesh file path specified in the URDF model file (*) | -| | fileName | 'model.urdf' | File name of the URDF model | -| | N_DOF | double | - | -| | N_DOF_MATRIX | [23×23 double] | - | -| | initialConditions | [1×1 struct] | Base pose and velocity. Joint positions and velocities | -| | SIMULATE_MOTOR_REFLECTED_INERTIA | 1 logical | Activate motor reflected inertia emulation | -| | robotFrames | [1×1 struct] | Selected reference frames (base, left/right sole) | -|contact_config| foot_print | [3×4 double] | 4 Contact points on one of the feet soles | -| | total_num_vertices | Nv | Total number of contact points (Nv=8) | -| | friction_coefficient | 1 double | Ground/feet Coulomb friction coefficient $\mu _{xy}$ (F _{\bot} = \mu _{xy} N) | -|physics_config| GRAVITY_ACC | [1x3 double] | Gravity vector (gz = -9.81) | -| | TIME_STEP | 1 double | Simulator sampling time (recommended 1e-03) | +|robot_config| jointOrder | [1×N_DOF] / cell | List of N_DOF "controlled" joints (matches dimension of joint torques input) | +| | meshFilePrefix | char | Prefix to be concatenated to the mesh file path specified in the URDF model file (*) | +| | fileName | char | File name of the URDF model (typically: model.urdf) | +| | N_DOF | [1x1] double | Robot degrees of freedom | +| | N_DOF_MATRIX | [23×23] double] | `eye(robot_config.N_DOF)` | +| | initialConditions | [1×1] struct | Base pose and velocity. Joint positions and velocities | +| | SIMULATE_MOTOR_REFLECTED_INERTIA | [1x1] logical | Activate motor reflected inertia emulation | +| | robotFrames | [1×1] struct | Selected reference frames (base, left/right sole) | +|contact_config| foot_print | [3×4] double | 4 Contact points on one of the feet soles | +| | total_num_vertices | [1x1] double | Total number of contact points (Nv=8) | +| | friction_coefficient | [1x1] double | Ground/feet Coulomb friction coefficient $\mu _{xy}$ (F _{\bot} = \mu _{xy} N) | +|physics_config| GRAVITY_ACC | [1x3] double | Gravity vector (gz = -9.81) | +| | TIME_STEP | [1x1] double | Simulator sampling time (recommended 1e-03) | ### Notes (*) Since iDynTree 3.0.0, if meshFilePrefix='', the standard iDynTree workflow of locating the mesh via the `ExternalMesh.getFileLocationOnLocalFileSystem` method is used. The iCub model meshes file tree is compatible with this workflow. -To run and example open and launch `test_matlab_system.mdl`. It also contains a callback to the `init` file that retrieves the needed information. +4. Set to `vector` or `matrix` the parameter "input motor reflected inertia format" from the pop-up pick-list. The selected format should match the format of the input signal `motorInertias` and respective definition as follows: + +| parameter value | `motorInertias` format | represented quantity | +| --- | --- | --- | +| vector | [NDOFx1] | Column vector composed of the diagonal terms of $\Gamma^{-\top} G^{-1} I _m G^{-1} \Gamma^{-1}$ | +| matrix | [NDOFxNDOF] | Matrix defined by $\Gamma^{-\top} G^{-1} I _m G^{-1} \Gamma^{-1}$ | + +The motor inertia reflected on the mass matrix of the articulated system is given by $\Gamma^{-\top} G^{-1} I _m G^{-1} \Gamma^{-1}$, which results in a diagonal matrix in the case of a robot without coupled joints. In such case, we only need to pass to **RobotDynWithContacts** the diagonall terms as a [NDOFx1] column vector. Otherwise, in presence of coupled joints (as in the case of iCub), the coupling matrix $\Gamma$ is not diagonal (the gearbox ratio matrix $G$ is though). The same applies to the term $\Gamma^{-\top} G^{-1} I _m G^{-1} \Gamma^{-1}$, which now does not have only diagonal terms and has to be passed to the block **RobotDynWithContacts** in its full matrix form of dimension [N_DOFxN_DOF]. + + +5. To run and example open and launch `test_matlab_system.mdl`. It also contains a callback to the `init` file that retrieves the needed information.