Support for continuous joints

[IoannisDadiotis]

Hi @FrancescoRuscelli ,

I have the above minimal piece of code, trying to create an optimal control problem for Centauro using horizon (code copied from spot_walk.py file).

import time
from typing import List
from horizon import problem
from horizon.utils import utils, kin_dyn, plotter, mat_storer
from casadi_kin_dyn import pycasadi_kin_dyn as cas_kin_dyn
from horizon.solvers import solver
import os, argparse
import numpy as np
import casadi as cs


path_to_centauro = '/home/idadiotis/forest_ws/src/iit-centauro-ros-pkg/centauro_urdf'
urdffile = os.path.join(path_to_centauro, 'urdf', 'centauro.urdf')
urdf = open(urdffile, 'r').read()
kindyn = cas_kin_dyn.CasadiKinDyn(urdf)

joint_names = kindyn.joint_names()
if 'universe' in joint_names: joint_names.remove('universe')
if 'floating_base_joint' in joint_names: joint_names.remove('floating_base_joint')

tf = 10.0
n_nodes = 100

# parameters
n_c = 4
n_q = kindyn.nq()
n_v = kindyn.nv()
n_f = 3
dt = tf / n_nodes
contacts_name = ['lf_foot', 'rf_foot', 'lh_foot', 'rh_foot']

# define dynamics
prb = problem.Problem(n_nodes)
q = prb.createStateVariable('q', n_q)
q_dot = prb.createStateVariable('q_dot', n_v)
q_ddot = prb.createInputVariable('q_ddot', n_v)
f_list = [prb.createInputVariable(f'force_{i}', n_f) for i in contacts_name]
x, x_dot = utils.double_integrator_with_floating_base(q, q_dot, q_ddot)
prb.setDynamics(x_dot)
prb.setDt(dt)

I am getting the following error:

Traceback (most recent call last):
  File "/home/idadiotis/centauro_ws/src/try_horizon/src/test.py", line 39, in <module>
    prb.setDynamics(x_dot)
  File "/usr/local/lib/python3.8/dist-packages/casadi_horizon-0.4.0-py3.8-linux-x86_64.egg/horizon/problem.py", line 205, in setDynamics
    raise ValueError(f'state derivative dimension mismatch ({xdot.shape[0]} != {nx})')
ValueError: state derivative dimension mismatch (95 != 99)

The joint_names list has length 42 and kindyn object gives nq = 52, nv = 47.
I believe the difference in the dimension between state and state derivative is due to the Pinocchio representation of continuous joints (the 4 wheels of Centauro). In particular, for each continuous joint Pinocchio adds 2 elements in the configuration vector (cos(theta). sin(theta)) and one element(d(theta)/dt) in the velocity vector.

If the above are true, it should not be hard to modify utils.double_integrator_with_floating_base to account for continuous joints.

[FrancescoRuscelli]

You are indeed right, thank you very much for the comment. We will fix this as soon as possible, as @alaurenzi suggested, letting Pinocchio manage the integration.
In the meantime, I suggest switching the joint type of the wheels from continuous to revolute. Setting huge lower and upper bounds would do the trick.