Kinematics.h

Go to the documentation of this file.

/*
 * RBDL - Rigid Body Dynamics Library
 * Copyright (c) 2011-2018 Martin Felis <martin@fysx.org>
 *
 * Licensed under the zlib license. See LICENSE for more details.
 */
 
#ifndef RBDL_KINEMATICS_H
#define RBDL_KINEMATICS_H
 
#include "rbdl/rbdl_math.h"
#include <assert.h>
#include <iostream>
#include "rbdl/Logging.h"
 
namespace RigidBodyDynamics {
 
RBDL_DLLAPI void UpdateKinematics (Model &model,
    const Math::VectorNd &Q,
    const Math::VectorNd &QDot,
    const Math::VectorNd &QDDot
    );
 
RBDL_DLLAPI void UpdateKinematicsCustom (Model &model,
    const Math::VectorNd *Q,
    const Math::VectorNd *QDot,
    const Math::VectorNd *QDDot
    );
 
RBDL_DLLAPI Math::Vector3d CalcBodyToBaseCoordinates (
    Model &model,
    const Math::VectorNd &Q,
    unsigned int body_id,
    const Math::Vector3d &body_point_position,
    bool update_kinematics = true);
 
RBDL_DLLAPI Math::Vector3d CalcBaseToBodyCoordinates (
    Model &model,
    const Math::VectorNd &Q,
    unsigned int body_id,
    const Math::Vector3d &base_point_position,
    bool update_kinematics = true);
 
RBDL_DLLAPI Math::Matrix3d CalcBodyWorldOrientation (
    Model &model,
    const Math::VectorNd &Q,
    const unsigned int body_id,
    bool update_kinematics = true);
 
RBDL_DLLAPI void CalcPointJacobian (Model &model,
    const Math::VectorNd &Q,
    unsigned int body_id,
    const Math::Vector3d &point_position,
    Math::MatrixNd &G,
    bool update_kinematics = true
    );
 
RBDL_DLLAPI void CalcPointJacobian6D (Model &model,
    const Math::VectorNd &Q,
    unsigned int body_id,
    const Math::Vector3d &point_position,
    Math::MatrixNd &G,
    bool update_kinematics = true
    );
 
RBDL_DLLAPI void CalcBodySpatialJacobian (
    Model &model,
    const Math::VectorNd &Q,
    unsigned int body_id,
    Math::MatrixNd &G,
    bool update_kinematics = true
    );
 
RBDL_DLLAPI Math::Vector3d CalcPointVelocity (
    Model &model,
    const Math::VectorNd &Q,
    const Math::VectorNd &QDot,
    unsigned int body_id,
    const Math::Vector3d &point_position,
    bool update_kinematics = true
    );
 
RBDL_DLLAPI
  Math::SpatialVector CalcPointVelocity6D (
      Model &model,
      const Math::VectorNd &Q,
      const Math::VectorNd &QDot,
      unsigned int body_id,
      const Math::Vector3d &point_position,
      bool update_kinematics = true
      );
 
RBDL_DLLAPI
  Math::Vector3d CalcPointAcceleration (
      Model &model,
      const Math::VectorNd &Q,
      const Math::VectorNd &QDot,
      const Math::VectorNd &QDDot,
      unsigned int body_id,
      const Math::Vector3d &point_position,
      bool update_kinematics = true
      );
 
RBDL_DLLAPI
  Math::SpatialVector CalcPointAcceleration6D (
      Model &model,
      const Math::VectorNd &Q,
      const Math::VectorNd &QDot,
      const Math::VectorNd &QDDot,
      unsigned int body_id,
      const Math::Vector3d &point_position,
      bool update_kinematics = true
      );
 
#ifndef RBDL_USE_CASADI_MATH
RBDL_DLLAPI
  bool InverseKinematics (
      Model &model,
      const Math::VectorNd &Qinit,
      const std::vector<unsigned int>& body_id,
      const std::vector<Math::Vector3d>& body_point,
      const std::vector<Math::Vector3d>& target_pos,
      Math::VectorNd &Qres,
      double step_tol = 1.0e-12,
      double lambda = 0.01,
      unsigned int max_iter = 55
      );
 
RBDL_DLLAPI Math::Vector3d CalcAngularVelocityfromMatrix (
    const Math::Matrix3d &RotMat);
 
struct RBDL_DLLAPI InverseKinematicsConstraintSet {
  enum ConstraintType {
    ConstraintTypePosition = 0,
    ConstraintTypeOrientation,
    ConstraintTypeFull,
    ConstraintTypePositionXY,
    ConstraintTypePositionZ,
    ConstraintTypePositionCoMXY
  };
 
  InverseKinematicsConstraintSet();
 
  Math::MatrixNd J; 
  Math::MatrixNd G; 
  Math::VectorNd e; 
 
  unsigned int num_constraints; //size of all constraints
  double lambda; 
  unsigned int num_steps; // The number of iterations performed
  unsigned int max_steps; // Maximum number of steps (default 300), abort if more steps are performed.
  double step_tol; // Step tolerance (default = 1.0e-12). If the computed step length is smaller than this value the algorithm terminates successfully (i.e. returns true). If error_norm is still larger than constraint_tol then this usually means that the target is unreachable.
  double constraint_tol; // Constraint tolerance (default = 1.0e-12). If error_norm is smaller than this value the algorithm terminates successfully, i.e. all constraints are satisfied.
  double error_norm; // Norm of the constraint residual vector.
  double delta_q_norm; //Norm of the change in generalized coordinates
 
  // everything to define a IKin constraint
  std::vector<ConstraintType> constraint_type;
  std::vector<unsigned int> body_ids;
  std::vector<Math::Vector3d> body_points;
  std::vector<Math::Vector3d> target_positions;
  std::vector<Math::Matrix3d> target_orientations;
  std::vector<unsigned int> constraint_row_index;
  std::vector<float> constraint_weight;
 
  // Adds a point constraint that tries to get a body point close to a 
  // point described in base coordinates.
  unsigned int AddPointConstraint (unsigned int body_id, const Math::Vector3d &body_point, const Math::Vector3d &target_pos, float weight = 1.);
  // Adds an orientation constraint that tries to align a body to the
  // orientation specified as a rotation matrix expressed in base
  // coordinates.
  unsigned int AddOrientationConstraint (unsigned int body_id, const Math::Matrix3d &target_orientation, float weight = 1.);
  // Adds a constraint on both location and orientation of a body.
  unsigned int AddFullConstraint (unsigned int body_id, const Math::Vector3d &body_point, const Math::Vector3d &target_pos, const Math::Matrix3d &target_orientation, float weight = 1.);
  // Clears all entries of the constraint setting
  unsigned int ClearConstraints(); 
  
  unsigned int AddPointConstraintXY (unsigned int body_id, const Math::Vector3d &body_point, const Math::Vector3d &target_pos, float weight = 1.);
  unsigned int AddPointConstraintZ  (unsigned int body_id, const Math::Vector3d &body_point, const Math::Vector3d &target_pos, float weight = 1.);
  unsigned int AddPointConstraintCoMXY (unsigned int body_id, const Math::Vector3d &target_pos, float weight = 1.);
 
};
 
RBDL_DLLAPI bool InverseKinematics (
    Model &model,
    const Math::VectorNd &Qinit,
    InverseKinematicsConstraintSet &CS,
    Math::VectorNd &Qres
    );
#endif
 
}
 
/* RBDL_KINEMATICS_H */
#endif