Classes for on-line solution of inverse kinematic of iCub limbs based on IPOPT. More...
Collaboration diagram for iKinSlv:
Classes | |
| class | iCub::iKin::CartesianSolver |
| Abstract class defining the core of on-line solvers. More... | |
| class | iCub::iKin::iCubArmCartesianSolver |
| Derived class which implements the on-line solver for the chain torso+arm. More... | |
| class | iCub::iKin::iCubLegCartesianSolver |
| Derived class which implements the on-line solver for the leg chain. More... | |
Detailed Description
Classes for on-line solution of inverse kinematic of iCub limbs based on IPOPT.
The solvers run as on-line daemons which connect to the robot to retrieve information on the current joints configuration (along with the bounds) and by requesting a desired pose with queries made through YARP ports they return the corresponding target joints.
The task to be solved is:
\[ \begin{array}{c} \mathbf{q}=\arg\min\limits_{\mathbf{q} \in \mathbb{R}^{n} }\left(\left\|\mathbf{\alpha}_d-\mathit{K_{\alpha}}\left(\mathbf{q}\right)\right\|^2+\mathit{w}\cdot\left(\mathbf{q}_r-\mathbf{q}\right)^{\top}\mathit{W}_r\left(\mathbf{q}_r-\mathbf{q}\right)\right) \\ \text{s.t.} \begin{cases} \left\|\mathbf{x}_d-\mathit{K_x}\left(\mathbf{q}\right)\right\|^2=0 \\ \mathbf{q}_L<\mathbf{q}<\mathbf{q}_U \end{cases} \end{array}. \]
Where the solution \( \mathbf{q} \) is the joints vector with n components (depending on the task and the current dof configuration) that is guaranteed to be found within the physical bounds expressed by \( \mathbf{q}_L \) and \( \mathbf{q}_U \); \( \mathbf{x}_d\equiv\left(x,y,d\right)_d \) is the positional part of the desired pose, \( \mathbf{\alpha}_d \) is the desired orientation and \( \mathit{K_x} \) and \( \mathit{K_{\alpha}} \) are the forward kinematic maps for the position and orientation part, respectively; \( \mathbf{q}_r \) is used to keep the solution as close as possible to a given rest position in the joint space (weighting with a positive factor \( \mathit{w} < 1 \) and also through the diagonal matrix \( \mathit{W}_r \) which allows selecting a specific weight for each joint).
Solver protocol
Once created the solver will open three ports with which user can communicate and exchange information according to the following rules:
/<solverName>/in accepts a properties-like bottle containing the following requests in streaming mode:
xd request: example ([xd] (x y z ax ay az theta)), specifies the target pose to be achieved as a 7-components vector for position and orientation parts. Note that if the current pose is of [xyz] type, then only the first 3-components are meaningful (x,...,az are in meters, theta is in radians).
pose request: example ([pose] [full]), specifies the end-effector pose the user wants to achieve; it can be [full] (position+orientation) or [xyz] (only position).
mode request: example ([mode] [cont]), selects the solver mode between [cont] which implements a continuous tracking and [shot] which does not compensate for movements induced on unacatuated joints. Indeed, in continuous mode the solver yields a new solution whenever a detected movements on uncontrolled links determines a displacement from the desired target.
dof request: example ([dof] (1 1 0 1 ...)), specifies which dof of the chain are actuated (by putting 1 in the corresponding position) and which not (with 0). The length of the provided list of 1's and 0's should match the number of chain's dof. The special value 2 is used to keep the link status unchanged and proceed to the next link.
resp request: example ([resp] (20.0 0.0 0.0 ...)), specifies for each joint the rest position in degrees. The reply will contain the rest position resulting from the application of joints limits.
resw request: example ([resw] (1.0 0.0 1.0 ...)), specifies for each joint the weight used to compute the rest position minimization task. The reply will contain the weights as result of a saturation over zero.
tok synchro: a double may be added to the request which will be in turn sent back by the solver for synchronization purpose.
- Note
- One single command sent to the streaming input port can contain multiple requests: e.g. ([pose] [xyz]) ([dof] (1 0 1 2 1)) ([xd] (-0.1 0.1 0.1) ([tok] 1.234) ...)
- Remind that the intended use of this port is in streaming mode, thus it might happen that one request is dropped in favour of the most recent one. Therefore, as a general remark, rely on this port for xd requests and use rpc for dof, mode, pose, ... requests (see below).
/<solverName>/rpc accepts a vocab-like bottle containing the following requests, executes them and replies with [ack]/[nack] and/or some useful info:
Commands issued through [set]/[get] vocab:
pose request: example [set] [pose] [full]/[xyz], [get] [pose].
pose priority request: example [set] [prio] [xyz]/[ang], [get] [prio]. For example, setting priority to [ang] allows considering the reaching in orientation as a constraint while reaching in position is handled as an objective.
- Note
- Caveat. If priority is given to [ang], then commands such as [set] [pose] will need still to be used with the tag [xyz] in order to reach for a non-full pose. This choice, even if counter-intuitive, ensures back-compatibility and in the end does not represent a big hitch since orientation is very rarely prioritized over position.
mode request: example [set] [mode] [cont]/[shot], [get] [mode].
lim request: example [set] [lim] axis min max, [get] [lim] axis. Set/return minimum and maximum values for the joint. Allowed range shall be a valid subset of the real control limits (unit is deg).
verbosity request: example [set] [verb] [on]/[off], [get] [verb].
dof request: example [set] [dof] (1 2 1 0 ...), [get] [dof]. The reply will contain the current dof as result of the reconfiguration. The result may differ from the request since on certain limb (e.g. arm) some links are considered to form a unique super-link (e.g. the shoulder) whose components cannot be actuated separately.
resp request: example [set] [resp] (20.0 0.0 0.0 ...), [get] [resp]. Set/get for each joint the rest position in degrees. The reply will contain the rest position resulting from the application of joints limits (lim request does affect the range).
resw request: example [set] [resw] (1.0 0.0 1.0 ...), [get] [resw]. Set/get for each joint the weight used to compute the rest position minimization task. The reply will contain the weights as result of a saturation over zero.
tsk2 request: example [set] [tsk2] (6 (0.0 0.0 -1.0) (0.0 0.0 1.0), [get] [tsk2]. Set/get the options for the secondary task, where the first integer accounts for the joints to control, then come the desired x-y-z coordinates of the secondary end-effector and finally the corresponding three weights.
conv request: example [set] [conv] ((max_iter 200) (tol 0.001)), [get] [conv]. Set/get the options for specifying solver's convergence.
Commands issued through the [ask] vocab:
xd request: example [ask] ([xd] (x y z ax ay az theta)) ([pose] [xyz]) ([q] (...)). Ask to solve for the target xd. User can specifies a different starting joint configuration q and the pose mode. The reply will contain something like [ack] ([q] (...)) ([x] (...)), where the found configuration q is returned as well as the final attained pose x.
Commands concerning the thread status:
susp request: example [susp], suspend the thread.
run request: example [run], let the thread run again.
status request: example [status], returns [ack] "not_started"|"running"|"suspended".
quit request: example [quit], close ports and quit the thread. [ack] is returned.
Commands concerning the solver configuration:
cfg request: example [cfg] (robot icub) (type left) (pose full) (tol 0.001) (constr_tol 0.000001) (maxIter 150) ... Once instantiated the solver is not automatically configured. To do that user can call the open() method locally or can configure the solver remotely by sending to the rpc port the configuration properties. For a list of these properties please see the documentation of open() method.
/<solverName>/out streams out a bottle containing the result of optimization instance. The format is ([xd] (...)) ([x] (...)) ([q] (...) ([tok] ...)) as following:
xd property: contains the desired cartesian pose to be achieved (7-components vector).
x property: contains the real cartesian pose achieved which in norm is the nearest one to xd (7-components vector).
q property: contains the joints configuration which achieves x (DOF-components vector in degrees).
tok property: contains the token that the client may have added to the request.
Date: first release 20/06/2009
