This PR adds the class DQ_FreeFlyingRobot()

[juanjqo]

@dqrobotics/developers

Hi @mmmarinho,

This PR adds the class DQ_FreeFlyingRobot(), which is a pending task discussed here. The class implements the following methods:

    DQ fkm(const DQ& pose) const;
    MatrixXd pose_jacobian(const DQ& pose) const;
    MatrixXd pose_jacobian_derivative(const DQ& pose_derivative) const;

The virtual methods from DQ_Kinematics were implemented by throwing exceptions since this class (to my best knowledge) is not compatible with these signatures.

//Virtual method overloads (DQ_Kinematics)
    virtual DQ fkm                (const VectorXd&) const override;
    virtual DQ fkm                (const VectorXd&, const int&) const override;
    virtual MatrixXd pose_jacobian(const VectorXd&, const int&) const override;
    virtual MatrixXd pose_jacobian_derivative(const VectorXd& q,
                                              const VectorXd& q_dot,
                                              const int& to_ith_link) const override;

Best regards,

Juancho

TODO

There are additional tasks required for this PR to be accepted (I'm working on it):

• Add the class DQ_FreeFlyingRobot() in dqrobotics/cpp.
• Add an example in cpp/examples. Added here
• Add the class in dqrobotics/python. Added here
• Add a test in python/tests. Added here

[mmmarinho]

@juanjqo Sorry for the delay, it seems that some points need reflection before we move on with this one. In particular, the lack of compatibility between this class and the other derived classes of DQ_Kinematics.

You might also want to consider how this class would play with the DQ_Controller classes with @bvadorno. I think this might be the main point my main concern right now when considering the compatibility between classes.

[mmmarinho]

You can add that documentation @bvadorno mentioned in MATLAB, as it might be useful here too.
It can be on the description of the method, after the @brief.

        % This robot configuration is given by the unit dual quaternion
        % that represents it's pose. Although strictly speaking this is
        % a 6-dimensional manifold, here we're counting the numbers of
        % coefficients in the unit dual quaternion. Recall that a unit
        % dual quaternion has two constraints, therefore even though it
        % has eight coefficients, these two constraints decreases the
        % number of dimensions of the underlying manifold.

[juanjqo]

@mmmarinho thank you for your feedback. I'm going to implement the required modifications once the @dqrobotics/developers team reaches an agreement or consensus about the DQ_FreeFlyingRobot() class.

[mmmarinho]

Same for the comment below. However, this method is in CPP currently NOT compatible with the other derived classes. Would it instead be expecting a VectorXd as input that, then, is converted to a DQ? You might want to discuss this with @bvadorno .

% calculates the mobile robot pose Jacobian. If an index is
% passed as an argument, just ignore it because there is
% only one element in the chain. However, we have to tackle
% this case to ensure compatibility with other
% DQ_Kinematics objects.

[mmmarinho]

This it not yet available in MATLAB? It needs to show up there first.

[juanjqo]

@mmmarinho No, it's not. Like several implementations related to Jacobian derivatives that are currently implemented only in the C++ version, I added it and proposed a numerical test in ` in this PR. However, I understand your point. I'm going to propose the pose_jacobian_derivative method in Matlab first, as suggested. =)

[bvadorno]

Hi @juanjqo and @mmmarinho,

This is undesirable. Why are those implementations appearing in the C++ version and not in MATLAB? This only causes confusion and potential for code divergence in the long run.

All the best,
Bruno

[juanjqo]

Hi @bvadorno, I agree. Several methods were added to the C++ (master branch) version in the last months for internal developments. Some of them are related to the Jacobian derivatives. This indeed created a divergence between both versions of the DQ Robotics. However, this issue was discussed internally in our meetings, and we defined several tasks to fix it. Such tasks are summarized in the Pending tasks for the 23.04 release discussions. Furthermore, it was highlighted that new methods must first been added/proposed in Matlab.

Best regards,

Juancho

[bvadorno]

Hi @juanjqo,

True! Thank you for refreshing my memory. 😃

Kind regards,
Bruno

[mmmarinho]

See my comment for pose_jacobian

[mmmarinho]

@dqrobotics/developers To give a concrete point of discussion, see the code draft below.
Please suppose that it compiles, I've just written it on this comment directly as food for thought.

Edit
Some points to consider

1. Would you want something like this to work? Indeed, they are both derived classes of DQ_Kinematics, so by subclass polymorphism this is somewhat expected.
2. If yes, in what shape/form/to what extent?
3. If DQ_FreeFlyingRobot works in a different way than other derived classes of DQ_Kinematics, how to support it with our controllers?

(slight modification of the qpoases_example)

#include <iostream>

#include <dqrobotics/DQ.h>
#include <dqrobotics/robot_modeling/DQ_FreeFlyingRobot.h>
#include <dqrobotics/robots/KukaLw4Robot.h>
#include <dqrobotics/solvers/DQ_QPOASESSolver.h>
#include <dqrobotics/robot_control/DQ_ClassicQPController.h>
#include <dqrobotics/utils/DQ_Constants.h>

using namespace DQ_robotics;

int main(void)
{
    // Define robot 1
    auto robot_1 = std::make_shared<DQ_SerialManipulatorDH>(KukaLw4Robot::kinematics());
    VectorXd q_init_1 = VectorXd::Zero(7);
    VectorXd q_d_1 = VectorXd::One(7);
 
    // Define robot 2
    auto robot_2 = std::make_shared<DQ_FreeFlyingRobot>();
    VectorXd q_init_2 = (VectorXd(8) << 1,0,0,0,0,0,0,0).finished();
    VectorXd q_d_2 = (VectorXd(8) << 0,1,0,0,0,0,0,0).finished();

    // Put them in a std::vector
    std::vector<std::shared_ptr<DQ_Kinematics>> robots{robot_1, robot_2};
    std::vector<VectorXd> q_inits{q_init_1, q_init_2};
    std::vector<VectorXd> q_ds{q_d_1, q_d_2};

   // Loop through the robots, calling DQ_Kinematics methods.
   for(auto i=0; i<robots.size(); I++)
   {
      auto& robot = robots[i];
      auto& q_init = q_inits[i];
      auto& q_d = q_ds[i];
     
      auto qpoases_solver = std::make_shared<DQ_QPOASESSolver>();
      DQ_ClassicQPController classic_qp_controller(robot,qpoases_solver);
      classic_qp_controller.set_control_objective(ControlObjective::Pose);
      classic_qp_controller.set_gain(0.01);
      classic_qp_controller.set_damping(0.01);
  
      DQ x = robot->fkm(q_init); 
      DQ xd = robot->fkm(q_d);
  
      VectorXd q_dot = classic_qp_controller.compute_setpoint_control_signal(q_init,vec8(xd));
  
      std::cout << "q_dot is " << q_dot.transpose() << std::endl;
   }

    return 0;
}

[ffasilva]

Hi, @dqrobotics/developers,

The way I see it, the DQ_FreeFlyingRobot() should work as any other kinematic chain does in the library. That is, to do something like what was proposed in @mmmarinho's example, robot_2 should be used as the base for the serialization with robot_1 in the class DQ_SerialWholeBody. So we would have:

//  // Put them in a std::vector
//  std::vector<std::shared_ptr<DQ_Kinematics>> robots{robot_1, robot_2};
//  std::vector<VectorXd> q_inits{q_init_1, q_init_2};
//  std::vector<VectorXd> q_ds{q_d_1, q_d_2};

// This should replace the "Put them in a std::vector"
free_flying_manipulator = DQ_SerialWholeBody(robot_2);
free_flying_manipulator.add(robot_1);

After that, we should be able to perform the control with any of the available controllers. For instance:

// Loop through the robots, calling DQ_Kinematics methods.

//  ... variable declarations ...

DQ_ClassicQPController classic_qp_controller(free_flying_manipulator, qpoases_solver);
classic_qp_controller.set_control_objective(ControlObjective::Pose);
classic_qp_controller.set_gain(0.01);
classic_qp_controller.set_damping(0.01);

// rest of the loop "// Loop through the robots, calling DQ_Kinematics methods."

My suggestion would be to make class DQ_FreeFlyingRobot: public DQ_MobileBase and adapt from here. I think we should keep any mobile robot as a subclass of DQ_MobileBase (and any fixed-base manipulator as a subclass of DQ_SerialManipulator) until we have strong reasons to change that.

Kind regards,
Frederico

[mmmarinho]

@ffasilva thanks for the input. @dqrobotics/developers

I think that discussion started by @ffasilva is very meaningful, but just so that we don't lose focus of the main point I was concerned about, let me put this into perspective.

The controllers receive a DQ_Kinematics and a DQ_Solver. Changing the DQ_FreeFlyingRobot to inherit from another derived class of DQ_Kinematics doesn't change/address the main point I wanted to make, because the behavior is unchanged. What @ffasilva is, of course, relevant still.

That aside, what we need (If I'm correct???) is to have an implementation of DQ_FreeFlyingRobot that allows the use of the controllers properly. (The current one in CPP doesn't).

---

Controller Implementation (the way it's always been)

(Using raw pointers for clarity).

DQ_ClassicQPController classic_qp_controller(DQ_Kinematics*, DQ_Solver*);

So, the DQ_FreeFlyingRobot would need to work with methods such as

cpp/src/robot_control/DQ_ClassicQPController.cpp

Lines 41 to 44 in 77acf9a

	MatrixXd DQ_ClassicQPController::compute_objective_function_symmetric_matrix(const MatrixXd &J, const VectorXd&)
	{
	return (J.transpose()*J + damping_*MatrixXd::Identity(_get_robot_ptr()->get_dim_configuration_space(),_get_robot_ptr()->get_dim_configuration_space()));
	}

Where the Jacobian is retrieved as follows.

cpp/src/robot_control/DQ_KinematicController.cpp

Lines 98 to 106 in 77acf9a

	MatrixXd DQ_KinematicController::get_jacobian(const VectorXd &q) const
	{
	DQ_Kinematics* robot_local =_get_robot_ptr();
	if(q.size() != robot_local->get_dim_configuration_space())
	throw std::runtime_error("Calling get_jacobian with an incorrect number of joints " + std::to_string(q.size()));
	const MatrixXd J_pose = robot_local->pose_jacobian(q);
	const DQ x_pose = robot_local->fkm(q);

---

Current proposal for DQ_FreeFlyingRobot, which does not seem compatible with this

(The pretty markdown preview doesn't work for these apparently)

https://github.com/juanjqo/cpp/blob/488c27650c529abae65097140c7028a1e4b03af4/src/robot_modeling/DQ_FreeFlyingRobot.cpp#L77

The main point is the change in signature, innocuous in MATLAB, but that generates utter havoc in CPP.

DQ DQ_FreeFlyingRobot::fkm(const DQ& pose) const
MatrixXd DQ_FreeFlyingRobot::pose_jacobian(const DQ &pose) const

---

Another point to consider

If the controller would be, after that is fixed, compatible.
I haven't worked on these before, so I'm not the expert here.

See the related issue: dqrobotics/matlab#73.

---

In concrete terms:

In addition to what has been done already, make sure that

• DQ_FreeFlyingRobot is compatible with DQ_KinematicController, e.g. DQ_ClassicQPController . Ideally, with a working minimal example. Or discuss why it shouldn't be, possible workarounds etc. (discussion started by @mmmarinho )
• We define what class DQ_FreeFlyingRobot should inherit from (discussion started by @ffasilva )

PPS: In philosophical terms

• Do we want all DQ_Kinematics derived classes to be compatible with DQ_KinematicController?
  or
• not and handle these corner cases somehow?

---

PS, another incompatible controller

A few years ago (see issue below), we noticed that another derived class of DQ_Kinematics, DQ_DifferentialDriveRobot, has an implementation that makes it incompatible with DQ_KinematicController . Just extra information for further reflection.

dqrobotics/python#23

[marcos-pereira]

Hi DQ robotics developers,

I am accompanying this discussion because the DQ_FreeFlyingRobot in C++ may be pretty useful for me. I will not get in details about the implementation itself, but as @mmmarinho said, the current constrained controller does not work directly with the free-flying robot. The discussion at #73 is a starting point, but it is not complete. The correct way to use the constrained controller with the free-flying robot requires additional math steps. It requires a different Jacobian and some adaptions in the constraints. I implemented these additional steps, which were provided by @bvadorno , but I believe they have not been published anywhere yet (although they are not any novelty).

If you like, once the architecture for the DQ_FreeFlyingRobot is complete, I could implement the steps to use the constrained controller with the free-flying robot. Else, you could also talk with @bvadorno about these details.

Kind regards,
Marcos

[bvadorno]

@ffasilva thanks for the input. @dqrobotics/developers

I think that discussion started by @ffasilva is very meaningful, but just so that we don't lose focus of the main point I was concerned about, let me put this into perspective.

The controllers receive a DQ_Kinematics and a DQ_Solver. Changing the DQ_FreeFlyingRobot to inherit from another derived class of DQ_Kinematics doesn't change/address the main point I wanted to make, because the behavior is unchanged. What @ffasilva is, of course, relevant still.

That aside, what we need (If I'm correct???) is to have an implementation of DQ_FreeFlyingRobot that allows the use of the controllers properly. (The current one in CPP doesn't).

Controller Implementation (the way it's always been)

(Using raw pointers for clarity).

DQ_ClassicQPController classic_qp_controller(DQ_Kinematics*, DQ_Solver*);

So, the DQ_FreeFlyingRobot would need to work with methods such as

cpp/src/robot_control/DQ_ClassicQPController.cpp

Lines 41 to 44 in 77acf9a

	MatrixXd DQ_ClassicQPController::compute_objective_function_symmetric_matrix(const MatrixXd &J, const VectorXd&)
	{
	return (J.transpose()*J + damping_*MatrixXd::Identity(_get_robot_ptr()->get_dim_configuration_space(),_get_robot_ptr()->get_dim_configuration_space()));
	}

Where the Jacobian is retrieved as follows.

cpp/src/robot_control/DQ_KinematicController.cpp

Lines 98 to 106 in 77acf9a

	MatrixXd DQ_KinematicController::get_jacobian(const VectorXd &q) const
	{
	DQ_Kinematics* robot_local =_get_robot_ptr();
	if(q.size() != robot_local->get_dim_configuration_space())
	throw std::runtime_error("Calling get_jacobian with an incorrect number of joints " + std::to_string(q.size()));
	const MatrixXd J_pose = robot_local->pose_jacobian(q);
	const DQ x_pose = robot_local->fkm(q);

Current proposal for DQ_FreeFlyingRobot, which does not seem compatible with this

(The pretty markdown preview doesn't work for these apparently)

https://github.com/juanjqo/cpp/blob/488c27650c529abae65097140c7028a1e4b03af4/src/robot_modeling/DQ_FreeFlyingRobot.cpp#L77

The main point is the change in signature, innocuous in MATLAB, but that generates utter havoc in CPP.

DQ DQ_FreeFlyingRobot::fkm(const DQ& pose) const
MatrixXd DQ_FreeFlyingRobot::pose_jacobian(const DQ &pose) const

Another point to consider

If the controller would be, after that is fixed, compatible. I haven't worked on these before, so I'm not the expert here.

See the related issue: dqrobotics/matlab#73.

In concrete terms:

In addition to what has been done already, make sure that

* [ ]  `DQ_FreeFlyingRobot` is compatible with `DQ_KinematicController`, e.g. `DQ_ClassicQPController `. Ideally, with a working minimal example. Or discuss why it shouldn't be, possible workarounds etc. (discussion started by @mmmarinho )

* [ ]  We define what class `DQ_FreeFlyingRobot` should inherit from (discussion started by @ffasilva )

PPS: In philosophical terms

* [ ]  Do we want all `DQ_Kinematics` derived classes to be compatible with `DQ_KinematicController`?
  or

* [ ]  not and handle these corner cases somehow?

PS, another incompatible controller

A few years ago (see issue below), we noticed that another derived class of DQ_Kinematics, DQ_DifferentialDriveRobot, has an implementation that makes it incompatible with DQ_KinematicController . Just extra information for further reflection.

dqrobotics/python#23

Hi @mmmarinho,

I'm catching up on this issue now and starting from your summary. Conceptually speaking, both points you and @ffasilva raised are valid when considering how the library should work.

Now, I need to return to my initial implementation to see how easy it'll be to reconcile all those criteria. This is because our kinematic controllers do not usually respect the manifold. After all, they enforce the desired closed-loop error dynamics in the Euclidean space ($\dot{\underline{\boldsymbol{x}}} + \lambda\underline{\boldsymbol{x}}=0$), which works because we project the control signals to the manifold through the Jacobian. This does not happen when dealing with free-flying robots. Hence, we need the projections @marcos-pereira mentioned when using DQ_FreeFlyingRobot.

Let's put this PR on hold until I finish working on those points.

All the best,
Bruno

[mmmarinho]

Hello @bvadorno

Thank you. My very general guess would be to try keeping pose_jacobian for all subclasses compatible with the controllers in Euclidean space and give a new signature for Jacobians compatible with controllers that respect the manifold, e.g. pose_XXX_jacobian.

In this case, for the DQ_FreeFlyingRobot, the pose_jacobian would have the projections while pose_XXX_jacobian would not, hence one would not be stuck using projections when, as in the original use case you and @marcos-pereira explored, they do not need to.

Kind regards,
Murilo