CppAD::cg::CGException : GreaterThanZero cannot be called for non-parameters

[mohamdev]

Hello,

First I want to thank you for your awesome work, which is very useful to me. I'm trying to perform some code generation but I have the following exception :

terminate called after throwing an instance of 'CppAD::cg::CGException' what (): GreaterThanZero cannot be called for non-parameters, Aborted (core dumped)

This exception occurs only when I run the code, the latter is indeed compiled with no errors. Here is my code :

#include "pinocchio/parsers/urdf.hpp"
#include "pinocchio/algorithm/joint-configuration.hpp"
#include "pinocchio/algorithm/kinematics.hpp"
#include <iostream>
#include <urdf_parser/urdf_parser.h>
#include "pinocchio/codegen/cppadcg.hpp"
#include <boost/test/unit_test.hpp>
#include <boost/utility/binary.hpp>
#include <Eigen/Geometry>
// PINOCCHIO_MODEL_DIR is defined by the CMake but you can define your own directory here.
#ifndef PINOCCHIO_MODEL_DIR
#define PINOCCHIO_MODEL_DIR "path_to_the_model_dir"
#endif

using namespace CppAD;
using namespace CppAD::cg;
using namespace pinocchio;
typedef double Scalar;
typedef CppAD::cg::CG<Scalar> CGScalar;
typedef CppAD::AD<CGScalar> ADScalar;
typedef Eigen::Matrix<ADScalar,Eigen::Dynamic,Eigen::Dynamic> ADRotMat;
typedef Eigen::Matrix<ADScalar,Eigen::Dynamic,1> ADVector;

int main(int argc, char ** argv)
{

/**Generate a random robot ADScalar configuration ad_q from Scalarconfiguration q**/
typedef ADModel::ConfigVectorType ADConfigVectorType;
ADConfigVectorType ad_q(model.nq);
Model::ConfigVectorType q = pinocchio::randomConfiguration(model);
ad_q = q.cast<ADScalar>();

/**Generate independent variables X and start recording operations sequence**/
ADConfigVectorType & X = ad_q;
CppAD::Independent(X);

ADVector Quat_ad(4); //This is the ADScalar quaternion vector

/**Perform forward kinematics and get rotation matrix R of the End Effector**/
pinocchio::forwardKinematics(ad_model, ad_data, X); //Compute forward kinematics
ADRotMat R; R = ad_data.oMi[model.nq].rotation();     //Get rotation matrix of End Effector

/**Convert the rotation matrix into a Quaternion**/
rotMatToQuaternion(R, Quat_ad);

/**Stop recording and generate AD function**/
CppAD::ADFun<CGScalar> fun(X,Quat_ad);

/**Generate source code **/
CppAD::cg::ModelCSourceGen<Scalar> cgen(fun, "fkine_angular");
cgen.setCreateJacobian(true);
cgen.setCreateForwardZero(true);
cgen.setCreateForwardOne(true);
cgen.setCreateReverseOne(true);
cgen.setCreateReverseTwo(true);
CppAD::cg::ModelLibraryCSourceGen<Scalar> libcgen(cgen);

/**Compile source code **/
CppAD::cg::DynamicModelLibraryProcessor<Scalar> p(libcgen);
CppAD::cg::GccCompiler<Scalar> compiler;
std::unique_ptr<CppAD::cg::DynamicLib<Scalar>> dynamicLib = p.createDynamicLibrary(compiler);

}

I think the problem comes from the function rotMatToQuaternion() which converts a rotation matrix into a quaternion, but inside this function there are some GreaterThanZero() tests. Here is the function :

void rotMatToQuaternion(ADRotMat const & R, ADVector & Quat_ad){
    ADScalar tr = R(0,0) + R(1,1) + R(2,2);
    if (tr > 0){
        ADScalar S = 0.5 / sqrt(tr+1);
        Quat_ad(0) = (R(2,1) - R(1,2))*S;
        Quat_ad(1) = (R(0,2) - R(2,0))*S;
        Quat_ad(2) = (R(1,0) - R(0,1))*S;
        Quat_ad(3) = 0.25 / S;
    } else {
      if (R(0,0) > R(1,1) && R(0,0) > R(2,2)){
        ADScalar S = 2 * sqrt(1 + R(0,0) - R(1,1) - R(2,2));
        Quat_ad(0) = 0.25 * S;
        Quat_ad(1) = (R(0,1) + R(1,0)) / S;
        Quat_ad(2) = (R(0,2) + R(2,0)) / S;
        Quat_ad(3) = (R(2,1) - R(1,2)) / S;
      } else if (R(1,1) > R(2,2)) {
        ADScalar S = 2 * sqrt(1 + R(1,1) - R(0,0) - R(2,2));
        Quat_ad(0) = (R(0,1) + R(1,0)) / S;
        Quat_ad(1) = 0.25 * S;
        Quat_ad(2) = (R(1,2) + R(2,1)) / S;
        Quat_ad(3) = (R(0,2) - R(2,0)) / S;
      } else {
        ADScalar S = 2 * sqrt(1 + R(2,2) - R(0,0) - R(1,1));
        Quat_ad(0) = (R(0,2) + R(2,0)) / S;
        Quat_ad(1) = (R(1,2) + R(2,1)) / S;
        Quat_ad(2) = 0.25 * S;
        Quat_ad(3) = (R(1,0) - R(0,1)) / S;
      }
    }
}

As you can see, the GreaterThan tests are performed on the values of the rotation matrix, which are not a part of the independent variables vector X. I think that is probably why the exception says GreaterThanZero cannot be called for non parameters. Indeed, this rotation matrix R is calculated from the independent variables vector X using pinocchio::forwardKinematics(ad_model, ad_data, X);. And then, once the forward Kinematics performed, I get the rotation matrix using ADRotMat R; R = ad_data.oMi[model.nq].rotation();. Actually, I really need to convert the rotation matrix R into a quaternion because I want to compute the gradient of my Quaternion relatively to the vector X, that is why I have CppAD::ADFun<CGScalar> fun(X,Quat_ad). I hope I was clear enough, otherwise feel free to ask me further questions. Thank you again :)

[mohamdev]

I found the solution :) With CppAD, you have to overload the if statement, using CppAD::CondExpOp.

[FenglongSong]

@mohamdev Hi Adjel, thanks for your question and I'm facing a very similar question when I'm using the aba() function in Pinocchio. Would you mind telling me a bit more details about how to overload the if/else statements? I think these if/else branch are built-in parts of the functions inside pinocchio and I don't how to do the overloading. Your help will be much appreciated! Thanks in advance.

[bradbell]

@mohamdev Does this help: https://coin-or.github.io/CppAD/doc/condexp.htm

[FenglongSong]

@bradbell Thanks for your reply! I'm not an expert in CppAd, and I guess what you mean is I have to manipulate the function in which there is an if/else branch, writing it into something like result = CondExpOp(left, right, if_true, if_false). However, the if/else seems to be in a function inside a library called Pinocchio and I actually don't want to modify its source code to avoid unexpected errors. I don't know if my understanding is correct and please correct me if I was wrong.

Besides, here is @mohamdev 's question in Pinocchio GitHub issues: https://github.com/stack-of-tasks/pinocchio/issues/1142 . Seems he has solved the problem by using CppAD::CondExpOp.

[bradbell]

@FenglongSong My mistake, I should have directed my comment above to you and not @mohamdev . Yes it does seem to me that @mohamdev has converted his AD comparison operations to conditional expressions. I was just trying to provide clarity as to the purpose and use of conditional expressions.

[FenglongSong]

@bradbell ok I got it. This will be clear to be and thank you very much!