Finding feasible EE position with custom cost term

[awa152]

Hi,

my goal is to find an endeffector position, which is feasible and as close as possible to the target position.

I am wondering, if the results I get are due to a wrong implementation of my cost term or if it is due to the non-linearity of the forwards-kinematics or even solver specific. If I at least could exclude the first, that would help.

So basically I am trying to solve the inverse kinematics problem using the forwards kinematics equation to find a feasible EE position within the workspace of the manipulator. If the target given is not within the workspace, it should return the closest feasible position (probably not as straightforward as I thought it could be).

As I later on want to use the solution within my MPC, I added extra MPC control variables that represent the position of my joints and are constrained on the limits of said joints. I used your cost function term https://github.com/ethz-adrl/control-toolbox/blob/v3.0.2/ct_rbd/include/ct/rbd/robot/costfunction/TermTaskspaceGeometricJacobian.hpp and changed it to my needs:

  virtual double evaluate(const Eigen::Matrix<double, STATE_DIM, 1>& x, const Eigen::Matrix<double, CONTROL_DIM, 1>& u,
                          const double& t) override
  {
    // transform the robot state vector into a CT RBDState
    RBDState rbdState;
    rbdState.jointPositions() = u.template tail<KINEMATICS::NJOINTS>();  // x.template head<KINEMATICS::NJOINTS>();

    // position difference in world frame
    Eigen::Matrix<double, 3, 1> xCurr =
        kinematics_.getEEPositionInWorld(eeInd_, rbdState.basePose(), rbdState.jointPositions()).toImplementation();
    Eigen::Matrix<double, 3, 1> xDiff = xCurr - x_ref_;

    // compute the cost from the position error
    double pos_cost = 0.5 * (xDiff.transpose() * Q_pos_ * xDiff)(0, 0);

    return pos_cost;
  }

  //! compute derivative of this cost term w.r.t. the state
  virtual core::StateVector<STATE_DIM> stateDerivative(const core::StateVector<STATE_DIM>& x,
                                                       const core::ControlVector<CONTROL_DIM>& u,
                                                       const double& t) override
  {
    return core::StateVector<STATE_DIM>::Zero();
  }

  virtual core::ControlVector<CONTROL_DIM> controlDerivative(const core::StateVector<STATE_DIM>& x,
                                                             const core::ControlVector<CONTROL_DIM>& u,
                                                             const double& t) override
  {
    core::StateVector<CONTROL_DIM> grad;
    grad.setZero();

    RBDState rbdState;
    rbdState.jointPositions() = u.template tail<KINEMATICS::NJOINTS>();

    Eigen::Matrix<double, 6, KINEMATICS::NJOINTS> J = kinematics_.getJacobianBaseEEbyId(eeInd_, rbdState);
    Eigen::Matrix<double, 3, KINEMATICS::NJOINTS> J_pos = J.template bottomRows<3>();

    // position difference in world frame
    Eigen::Matrix<double, 3, 1> xCurr =
        kinematics_.getEEPositionInWorld(eeInd_, rbdState.basePose(), rbdState.jointPositions()).toImplementation();
    Eigen::Matrix<double, 3, 1> xDiff = xCurr - x_ref_;

    grad.segment(KINEMATICS::NJOINTS, (KINEMATICS::NJOINTS * 2) - 1) += J_pos.transpose() * Q_pos_ * xDiff;

    return grad;
  }

  virtual state_matrix_t stateSecondDerivative(const core::StateVector<STATE_DIM>& x,
                                               const core::ControlVector<CONTROL_DIM>& u, const double& t) override
  {
    return state_matrix_t::Zero();
  }

  virtual control_matrix_t controlSecondDerivative(const core::StateVector<STATE_DIM>& x,
                                                   const core::ControlVector<CONTROL_DIM>& u, const double& t) override
  {
    RBDState rbdState;
    rbdState.jointPositions() = u.template tail<KINEMATICS::NJOINTS>();

    Eigen::Matrix<double, 6, KINEMATICS::NJOINTS> J = kinematics_.getJacobianBaseEEbyId(eeInd_, rbdState);
    Eigen::Matrix<double, 3, KINEMATICS::NJOINTS> J_pos = J.template bottomRows<3>();

    control_matrix_t Q = control_matrix_t::Zero();
    Q.template bottomRightCorner<KINEMATICS::NJOINTS, KINEMATICS::NJOINTS>() += J_pos.transpose() * Q_pos_ * J_pos;

    return Q;
  }

Too summarize the code above, the only changes I did to the original file are using rbdState.jointPositions() = u.template tail<KINEMATICS::NJOINTS>(), to use the last control variables as joint positions. Also I set the derivatives of the states to zero and moved the code for the derivatives from here to the control derivatives. I think from the implementation side this should be correct?

The results can be seen in the short video I attached. The orange marker is the target marker and the green one is the current EE position, computed with the joint positions I get from the control variables. As soon as I click the target marker, the optimal solution is found right away. When I drag the target closer to the workspace border and beyond, the solver does not find a solution. I would like the marker to stay as close as possible to the target, but within the feasible workspace. Sometimes it seems to get stuck in a local minimum, while searching the solution space.

I use HPIPM and my solver settings are:

ilqr
{
    nlocp_algorithm ILQR
    integrator EulerCT
    useSensitivityIntegrator false
    discretization Forward_euler
    timeVaryingDiscretization false
    dt 0.03
    K_sim 1
    K_shot 1
    epsilon 0
    max_iterations 10
    fixedHessianCorrection false
    recordSmallestEigenvalue false
    min_cost_improvement 1e-5
    maxDefectSum 1e-5
    meritFunctionRho 0.0
    meritFunctionRhoConstraints 0.0
    nThreads 1
    nThreadsEigen 1
    printSummary true
    debugPrint false 
    
    line_search
    {
        type NONE
        adaptive flase
        maxIterations 10
        alpha_0 1.0
        n_alpha 0.5
        debugPrint true
    }
}

vid1.mp4

[markusgft]

Dear @awa152
nice video! I believe in principle we are facing the same problem here and in the other issue you opened.
In principle, the problem is ill conditioned in singularities, you may be seeing this effect here. But without analysing the morphology of your robot, it is hard to say if you are in fact hitting a singularity or not!