You signed in with another tab or window. Reload to refresh your session.You signed out in another tab or window. Reload to refresh your session.You switched accounts on another tab or window. Reload to refresh your session.Dismiss alert
Copy file name to clipboardExpand all lines: cartesian_vic_controller/README.md
+96-11Lines changed: 96 additions & 11 deletions
Original file line number
Diff line number
Diff line change
@@ -1,32 +1,53 @@
1
1
# cartesian_vic_controller
2
2
3
-
This is a cartesian VIC controller based on the package `admittance_controller` from [ros-controls/ros2_controllers](https://github.com/ros-controls/ros2_controllers/tree/master).
3
+
This is a cartesian VIC controller inspired by the package `admittance_controller` from [ros-controls/ros2_controllers](https://github.com/ros-controls/ros2_controllers/tree/master).
4
4
5
5
> **Warning**
6
6
>
7
7
> This package is currently under development and possibly unsafe if used to control an actual robot!
8
+
> For instance, a singularity avoidance strategy has still to be implemented in the default VIC.
8
9
9
10
## Control law
10
11
12
+
Let us consider a $n-DoF$ serial manipulator robot whose dynamics can be modeled as
where $H(q) \in \mathbb{R}^{n \times n}$ is the joint space inertia matrix, $C(\dot{q}, q) \in \mathbb{R}^{n}$ contains the coriolis and centrifugal terms, $G(q) \in \mathbb{R}^{n}$ represents the gravity terms, and $f_{ext}$ is the external wrench.
21
+
22
+
> [!NOTE]
23
+
> In the code as well as in the following, we define $J(q)$ as the "geometrical" Jacobian matrix, not the analytical one usually used in papers and textbooks.
24
+
> The reason is that we do not need/want to choose an angle parameterization (i.e., Euler, Yaw-Pitch-Roll, Quaternions, etc.) if not strictly required by the application.
25
+
> The orientation is provided to the controller in the form of a rotation matrix and we use angle-axis representation to compute orientation errors in the VIC rules.
26
+
> However, note that other rule implementations (e.g., QP, NMPC, etc.) will require an appropriate parameterization of the robot orientation and make the use of the geometrical Jacobian incorrect...
27
+
> For instance, see [this document](https://www.diag.uniroma1.it/deluca/rob2_en/15_ImpedanceControl.pdf) for reference.
28
+
11
29
The controller imposes the following cartesian behavior
12
30
13
31
$$
14
32
\begin{align}
15
-
M \ddot{e} + D \dot{e} + K e = - f_{ext}
33
+
M \ddot{e} + D \dot{e} + K e = f_{ext} - f_{ref}
16
34
\end{align}
17
35
$$
18
36
19
37
where $e = p^d - p$, $M$ and $D$ are the (positive definite) desired inertia and damping matrix, respectively, and $K^d$ is the (positive semi-definite) desired stiffness matrix.
38
+
The reference force $f_{ref}$ is the force that the robot will apply in the absence of interference with the K and D terms (i.e., if $K \tilde 0$, then $f_{ext} \tilde f_{ref}$ in permanent regime).
20
39
21
40
The cartesian acceleration command $\ddot{p}^c$ is therefore computed as
22
41
23
42
$$
24
43
\begin{align}
25
-
\ddot{p}^c = \ddot{p}^d + M^{-1} \left( D \dot{e} + K \dot{e} + f_{ext} \right)
44
+
\ddot{p}^c = \ddot{p}^d + M^{-1} \left( D \dot{e} + K \dot{e} - f_{ext} + f_{ref} \right)
26
45
\end{align}
27
46
$$
28
47
29
-
and integrated to obtain the cartesian velocity command $\dot{p}^c$.
48
+
### 1) Admittance control mode
49
+
50
+
The cartesian acceleration is integrated to obtain the cartesian velocity command $\dot{p}^c$.
30
51
31
52
The joint velocity command $\dot{q}^c$ is computed from $\dot{p}^c$ using the `kinematics_interface`.
32
53
@@ -40,13 +61,49 @@ $$
40
61
41
62
where $q$ is the **measured** joint position and $T_s$ is the controller sampling time.
42
63
64
+
See [vanilla_cartesian_admittance_rule](src/rules/vanilla_cartesian_admittance_rule.cpp) for implementation details.
65
+
66
+
### 2) Impedance control mode
67
+
68
+
The joint acceleration command is first computed as
Target (cartesian) compliant frame containing at least a pose, a twist.
48
103
49
-
Additionally, a desired compliance (stiffness, damping, inertia) can be provided for each reference cartesian state in the trajectory. Otherwise, the ros parameters are used. For instance you could provide the following ros parameters:
104
+
Additionally, a desired compliance (stiffness, damping, inertia) can be provided for each reference cartesian state in the trajectory.
105
+
Otherwise, the ros parameters are used to retrieve the default values.
106
+
For instance you could provide the following default impedance parameters:
50
107
51
108
```yaml
52
109
<controller_name>:
@@ -59,12 +116,10 @@ where $q$ is the **measured** joint position and $T_s$ is the controller samplin
0 commit comments