Unconstrained end-pose problem

The unconstrained end-pose problem (UnconstrainedEndPoseProblem) defines a problem minimizing the quadratic cost using a kinematic system. The state is represented by joint configurations and the output is a single configuration. The problem is formulated as:

\[\underset{\boldsymbol{x}}{\text{argmin}} (f(\boldsymbol{x})^\top Q f(\boldsymbol{x})).\]

The cost function is composed of weighted components:

\[f(\boldsymbol{x})=\sum_i \rho_i||\Phi_i(\boldsymbol{x})-\boldsymbol{y}^*_i||,\]

where \(\Phi_i(\boldsymbol{x})\) is the mapping function of the \(i\)-th task map, \(\boldsymbol{y}^*_i\) is the reference or goal in the task space, and \(\rho_i\) is the relative weighting of the task. By definition, This problem provides the first derivative of the quadratic cost function. The derivatives of the task terms are provided by the task map. Additionally, configuration space weighting \(W\) is specified. This allows us to scale the cost of moving each joint (or control variable) individually. Optionally, a nominal pose \(\boldsymbol{x}_\text{nominal}\) is provided as a reference often used to minimize secondary cost in case of redundancies. This type of problem can be used for solving inverse kinematics or to implement operational space control.

The example inverse kinematics problem is using this formulation. The full problem definition contains the definition of the planning scene and the task map as discussed previously. We also set the configuration space weighting \(W\). The problem is then fully defined using the following XML string:

<UnconstrainedEndPoseProblem Name="MyProblem">
  <PlanningScene>
    <Scene>
      <JointGroup>arm</JointGroup>
      <URDF>{exotica_examples}/resources/robots/lwr_simplified.urdf</URDF>
      <SRDF>{exotica_examples}/resources/robots/lwr_simplified.srdf</SRDF>
    </Scene>
  </PlanningScene>
  <Maps>
    <EffPosition Name="Position">
      <EndEffector>
          <Frame Link="lwr_arm_7_link" BaseOffset="0.5 0 0.5 0 0 0 1"/>
      </EndEffector>
    </EffPosition>
  </Maps>
  <W> 7 6 5 4 3 2 1 </W>
  <StartState>0 0 0 0 0 0 0</StartState>
  <NominalState>0 0 0 0 0 0 0</NominalState>
</UnconstrainedEndPoseProblem>

The weighting \(W\) is set to reduce movement of the joints closer to the root (root joint weight \(7\) to tip joint weight \(1\)). We set the start and the nominal configuration, or state, to a zero vector. This problem is now complete and we can use it to compute the robot configuration which moves the top of the robot to coordinates \((0.5, 0, 0.5)\). EXOTica provides several useful tools that make defining problems using XML more versatile. All strings are automatically parsed as the required data types. Furthermore, file paths containing curly bracket macros will be replaced with catkin package paths, e.g. {exotica_examples} will get replaced with the absolute path to the EXOTica package. This feature combines regular expressions with the rospack library to parse catkin package paths.