C++ Installation instructions

[mrunaljsarvaiya]

I'm trying to install the cpp bindings by following the cpp installation instructions here, but I run into this error when running cmake -DBUILD_WITH_PINOCCHIO=ON -DBUILD_WITH_qpOASES=ON .. I've installed all the dependencies so not sure why cmake can't find pybind11

Make Error at bindings/CMakeLists.txt:16 (find_package):
  By not providing "Findpybind11.cmake" in CMAKE_MODULE_PATH this project has
  asked CMake to find a package configuration file provided by "pybind11",
  but CMake did not find one.

  Could not find a package configuration file provided by "pybind11" with any
  of the following names:

    pybind11Config.cmake
    pybind11-config.cmake

  Add the installation prefix of "pybind11" to CMAKE_PREFIX_PATH or set
  "pybind11_DIR" to a directory containing one of the above files.  If
  "pybind11" provides a separate development package or SDK, be sure it has
  been installed.

-- Configuring incomplete, errors occurred!

[hungpham2511]

Can you have a look at the CI commands used to build the cpp library https://github.com/hungpham2511/toppra/blob/a70f6a217ddf20c42b62471ab52ce16884a320c1/.circleci/config.yml#L11

Have you install pybind11, and ensure that it is in CMAKE_PREFIX_PATH as suggested by the error message?

[mrunaljsarvaiya]

Yes I was missing some dependencies. I was able to install the cpp packages by running cmake -GNinja -DCMAKE_EXPORT_COMPILE_COMMANDS=1 -DBUILD_WITH_PINOCCHIO=ON -DBUILD_WITH_qpOASES=ON -DBUILD_WITH_GLPK=ON -DPYTHON_BINDINGS=ON -DPYBIND11_PYTHON_VERSION=3.7 ..

However, the command used in the CI doesn't work: cmake -DBUILD_WITH_PINOCCHIO=ON -DBUILD_WITH_qpOASES=ON -DBUILD_WITH_GLPK=ON -DPYTHON_BINDINGS=ON -DPYTHON_VERSION=3.8 -DBUILD_WITH_PINOCCHIO_PYTHON=true -DOPT_MSGPACK=ON -DBoost_NO_BOOST_CMAKE=ON -DCMAKE_EXPORT_COMPILE_COMMANDS=1 ..

I get this error CMake Error at /usr/share/cmake-3.16/Modules/FindPackageHandleStandardArgs.cmake:146 (message): Could NOT find Boost (missing: python3.8.10) (found version "1.71.0") eventhough DBoost_NO_BOOST_CMAKE is set to ON

[hungpham2511]

Based on https://github.com/hungpham2511/toppra/pull/146#issuecomment-964506349 you seem to have been able to install the cpp library and bindings?

[mrunaljsarvaiya]

I was able to install the bindings and am in the process of figuring out how to actually use CartesianVelocityNorm. However I can only get the system to compile without the BUILD_WITH_PINOCCHIO_PYTHON bindings. So I'm trying to figure out what's causing the build to fail if DBUILD_WITH_PINOCCHIO_PYTHON=ON

[hungpham2511]

I am not sure why it's failing either, CI seems to proceed nicely in both ubuntu 20.04 and 18.04

Do you have any error/screenshots?

[mrunaljsarvaiya]

I am not sure why it's failing either, CI seems to proceed nicely in both ubuntu 20.04 and 18.04

Do you have any error/screenshots?

I'm going to close this for now cause I landed up writing a python version of the cartesian constraint instead of linking the cpp bindings. I'll try and post some screenshots later this week in case someone else has the same issues. Thank you!

[RAJKgu]

@mrunaljsarvaiya Can you share the python implementation of the cartesian constraint or did you end updating the following implementation mentioned in #144 of the cartesian speed constraint

[mrunaljsarvaiya]

@RAJKgu Here's what I have, I used the joint_torque constraint as a reference

from .linear_constraint import LinearConstraint, canlinear_colloc_to_interpolate
from ..constraint import DiscretizationType
import numpy as np

class CartesianVelocityConstraint(LinearConstraint):
  """
  Cartesian Velocity Constraint
  """

  def __init__(
      self,
      jacobian_func,
      vlim_func,
      discretization_scheme=DiscretizationType.Collocation):
      super(CartesianVelocityConstraint, self).__init__()
      self.dof = 7
      self.set_discretization_type(discretization_scheme)
      self.identical = False
      self.jacobian_func = jacobian_func
      self.vlim_func = vlim_func

  def get_velocity_limit(self, i):
    """
    Use input limit handle to find cartesian eff limit
    """
    return self.vlim_func(i)

  def get_cartesian_velocity(self, q, qdot):
    """
    Use jacobian to find cartesian velocity
    """
    J = self.jacobian_func(list(q[:self.dof]))
    return J.dot(qdot[:self.dof])

  def compute_constraint_params(self, path, gridpoints):
    """
    path: :class:`Interpolator`
          The geometric path.
    gridpoints: np.ndarray
        Shape (N+1,). Gridpoint use for discretizing path.

    Returns
    -------
    a: np.ndarray or None
        Shape (N + 1, m). See notes.
    b: np.ndarray, or None
        Shape (N + 1, m). See notes.
    c: np.ndarray or None
        Shape (N + 1, m). See notes.
    F: np.ndarray or None
        Shape (N + 1, k, m). See notes.
    g: np.ndarray or None
        Shape (N + 1, k,). See notes
    ubound: np.ndarray, or None
        Shape (N + 1, 2). See notes.
    xbound: np.ndarray or None
        Shape (N + 1, 2). See notes.
    """

    # assert(self.dof == path.dof)

    N = gridpoints.shape[0]
    q = np.zeros((path.dof))
    qdot = np.zeros((path.dof))

    # Init params
    # m_limit is velocity limit in m/s
    # m_S decides how to find abs velocity given [v_x, v_y, v_z, w_x, w_y, w_z]

    # a,b,c and g are def vectors
    a = np.zeros((N))
    b = np.zeros((N))
    c = np.zeros((N))
    F = np.ones((N, 1, 1))
    g = np.zeros((N, 1,))

    q = path.eval(gridpoints)
    qdot = path.evald(gridpoints)

    # we only care about linear velocities into account
    # update as needed for your use case
    m_S = np.eye(6)
    m_S[2:,:] = 0

    for i in range(N):
      # Find new velocity limit
      # m_limit will get updated depending on gridpoint
      m_limit = self.get_velocity_limit(gridpoints[i])

      # Find cartesian velocity
      v = self.get_cartesian_velocity(q[i], qdot[i])

      # Update constraint params
      b[i] = np.sqrt(v.T.dot(m_S).dot(v))
      g[i] = m_limit

    if self.discretization_type == DiscretizationType.Collocation:
      # return a, None, None, None, None, None, None
      return a, b, c, F, g, None, None
    elif self.discretization_type == DiscretizationType.Interpolation:
      return canlinear_colloc_to_interpolate(
          a, b, c, F, g, None, None, gridpoints, identical=True
      )
    else:
      raise NotImplementedError("Other form of discretization not supported!")