internal::kinematics::InverseKinematicsData class
#include <private/InverseKinematicsData.h>

Constructors, destructors, conversion operators

InverseKinematicsData()

Public functions

void updateRobotConfiguration()
void prepareForOptimization()
void computeProblemSizeAndResizeBuffers()
void configureCenterOfMassProjectionConstraint()
auto setModel(const iDynTree::Model& model, const std::vector<std::string>& consideredJoints = std::vector<std::string>()) -> bool
auto setJointLimits(std::vector<std::pair<double, double>>& jointLimits) -> bool
auto getJointLimits(std::vector<std::pair<double, double>>& jointLimits) -> bool
void clearProblem()
auto addFrameConstraint(const internal::kinematics::TransformConstraint& frameTransformConstraint) -> bool
auto addTarget(const internal::kinematics::TransformConstraint& frameTransform) -> bool
auto getTargetRefIfItExists(const std::string targetFrameName) -> TransformMap::iterator
void updatePositionTarget(TransformMap::iterator target, iDynTree::Position newPos, double newPosWeight)
void updateRotationTarget(TransformMap::iterator target, iDynTree::Rotation newRot, double newRotWeight)
auto setRobotConfiguration(const iDynTree::Transform& baseConfiguration, const iDynTree::VectorDynSize& jointConfiguration) -> bool
auto setJointConfiguration(const std::string& jointName, const double jointConfiguration) -> bool
void setRotationParametrization(enum iDynTree::InverseKinematicsRotationParametrization parametrization)
auto rotationParametrization() -> enum iDynTree::InverseKinematicsRotationParametrization
void setDefaultTargetResolutionMode(enum iDynTree::InverseKinematicsTreatTargetAsConstraint mode)
auto defaultTargetResolutionMode() -> enum iDynTree::InverseKinematicsTreatTargetAsConstraint
void setTargetResolutionMode(TransformMap::iterator target, enum iDynTree::InverseKinematicsTreatTargetAsConstraint mode)
auto targetResolutionMode(TransformMap::iterator target) const -> enum iDynTree::InverseKinematicsTreatTargetAsConstraint
auto solveProblem() -> bool
auto dynamics() -> iDynTree::KinDynComputations&
void setCoMTarget(const iDynTree::Position& desiredPosition, double weight)
void setCoMasConstraint(bool asConstraint)
auto isCoMaConstraint() -> bool
void setCoMasConstraintTolerance(double TOL)
auto isCoMTargetActive() -> bool
void setCoMTargetInactive()

Public variables

bool isActive
iDynTree::Position desiredPosition
double weight
bool isConstraint
double constraintTolerance
iDynTree::VectorDynSize jointsConfiguration
iDynTree::Transform basePose
iDynTree::VectorDynSize jointsVelocity
iDynTree::Twist baseTwist
iDynTree::Vector3 worldGravity
std::vector<bool> fixedVariables
std::unordered_map<int, int> modelJointsToOptimisedJoints
iDynTree::Model reducedModel
bool m_problemInitialized
bool m_warmStartEnabled
size_t m_numberOfOptimisationVariables
size_t m_numberOfOptimisationConstraints
Ipopt::SmartPtr<Ipopt::IpoptApplication> m_solver
Ipopt::SmartPtr<internal::kinematics::InverseKinematicsNLP> m_nlpProblem

Function documentation

internal::kinematics::InverseKinematicsData::InverseKinematicsData()

Default constructor

void internal::kinematics::InverseKinematicsData::updateRobotConfiguration()

Update internal variables given a change in the robot state

void internal::kinematics::InverseKinematicsData::prepareForOptimization()

Prepare the internal data to run an optimization

void internal::kinematics::InverseKinematicsData::computeProblemSizeAndResizeBuffers()

compute the problem size (number of optimisation variables and constraints)

void internal::kinematics::InverseKinematicsData::configureCenterOfMassProjectionConstraint()

Configure the COM projection constraints given the current active contraints.

bool internal::kinematics::InverseKinematicsData::setModel(const iDynTree::Model& model, const std::vector<std::string>& consideredJoints = std::vector<std::string>())

Parameters
model the model to be used
consideredJoints
Returns true if successfull, false otherwise

Set the kinematic model to be used for the computations

bool internal::kinematics::InverseKinematicsData::setJointLimits(std::vector<std::pair<double, double>>& jointLimits)

Parameters
jointLimits vector of new joint limits to be imposed
Returns true if successfull, false otherwise

Set new joint limits

bool internal::kinematics::InverseKinematicsData::getJointLimits(std::vector<std::pair<double, double>>& jointLimits)

Parameters
jointLimits vector of current joint limits
Returns true if successfull, false otherwise

Get current joint limits

void internal::kinematics::InverseKinematicsData::clearProblem()

Reset variables to defaults

If the model has been loaded, defaults means #dofs size Otherwise the size is zero. All constraints are reset.

bool internal::kinematics::InverseKinematicsData::addFrameConstraint(const internal::kinematics::TransformConstraint& frameTransformConstraint)

Returns true if successfull, false otherwise

Add a constraint for the specified frame

bool internal::kinematics::InverseKinematicsData::addTarget(const internal::kinematics::TransformConstraint& frameTransform)

Parameters
frameTransform the frame to be considered as a target
Returns true if successfull, false otherwise

Add a target for the specified frame

TransformMap::iterator internal::kinematics::InverseKinematicsData::getTargetRefIfItExists(const std::string targetFrameName)

Get target if it exists.

Get a reference to a target if it exists, or return m_targets::end() print an error otherwise.

void internal::kinematics::InverseKinematicsData::updatePositionTarget(TransformMap::iterator target, iDynTree::Position newPos, double newPosWeight)

Update the position reference for a target

void internal::kinematics::InverseKinematicsData::updateRotationTarget(TransformMap::iterator target, iDynTree::Rotation newRot, double newRotWeight)

Update the position reference for a target

bool internal::kinematics::InverseKinematicsData::setRobotConfiguration(const iDynTree::Transform& baseConfiguration, const iDynTree::VectorDynSize& jointConfiguration)

Parameters
baseConfiguration base configuration (position and orientation)
jointConfiguration joints configuration
Returns true if successfull, false otherwise

Set the current robot configuration

This confguration will be used for all internal calls to kinematics functions

bool internal::kinematics::InverseKinematicsData::setJointConfiguration(const std::string& jointName, const double jointConfiguration)

Parameters
jointName name of the joint
jointConfiguration value of the joint
Returns true if successfull, false otherwise

Set the initial robot joint configuration for a specified joint

void internal::kinematics::InverseKinematicsData::setRotationParametrization(enum iDynTree::InverseKinematicsRotationParametrization parametrization)

Parameters
parametrization type of parametrization

Set the type of parametrization for the SO3 (rotation) Currently RPY and Quaternion are supported

enum iDynTree::InverseKinematicsRotationParametrization internal::kinematics::InverseKinematicsData::rotationParametrization()

Returns the current rotation parametrization

Return the current rotation parametrization used by the solver

void internal::kinematics::InverseKinematicsData::setDefaultTargetResolutionMode(enum iDynTree::InverseKinematicsTreatTargetAsConstraint mode)

Parameters
mode how to treat the targets

Set how targets should be considered in the optimization problem i.e. as soft or hard constraints. It applies only to the newly added targets

void internal::kinematics::InverseKinematicsData::setTargetResolutionMode(TransformMap::iterator target, enum iDynTree::InverseKinematicsTreatTargetAsConstraint mode)

Parameters
target
mode how to treat the target

Set how the specified target should be considered in the optimization problem i.e. as soft or hard constraints

enum iDynTree::InverseKinematicsTreatTargetAsConstraint internal::kinematics::InverseKinematicsData::targetResolutionMode(TransformMap::iterator target) const

Returns the resolution mode

Return the resolution mode adopted for the specified target

bool internal::kinematics::InverseKinematicsData::solveProblem()

Returns true if the problem is solved. False otherwise

Solve the NLP problem

iDynTree::KinDynComputations& internal::kinematics::InverseKinematicsData::dynamics()

Returns reference to the kinematics and dynamics object

Access the Kinematics and Dynamics object used by the solver

Variable documentation

iDynTree::VectorDynSize internal::kinematics::InverseKinematicsData::jointsConfiguration

joint configuration $ q_j \in \mathbb{R}^n $

iDynTree::Transform internal::kinematics::InverseKinematicsData::basePose

base position $ {}^w H_{base} \in SE(3) $

iDynTree::VectorDynSize internal::kinematics::InverseKinematicsData::jointsVelocity

joint velotiy $ \dot{q}_j \in \mathbb{R}^n $

iDynTree::Twist internal::kinematics::InverseKinematicsData::baseTwist

base velocity $ [\dot{p}, {}^I \omega] \in se(3) $

iDynTree::Vector3 internal::kinematics::InverseKinematicsData::worldGravity

gravity acceleration in inertial frame, i.e. -9.81 along z

Ipopt::SmartPtr<Ipopt::IpoptApplication> internal::kinematics::InverseKinematicsData::m_solver

Instance of IPOPT solver

iDynTree::KinDynComputations internal::kinematics::InverseKinematicsData::m_dynamics

object for kinematics and dynamics computation

std::vector<std::pair<double, double>> internal::kinematics::InverseKinematicsData::m_jointLimits

Limits for joints. The pair is ordered as min and max

struct internal::kinematics::InverseKinematicsData::@4 internal::kinematics::InverseKinematicsData::m_state

Variables needed to identify the state of the robot i.e. position and velocity

size_t internal::kinematics::InverseKinematicsData::m_dofs

internal DoFs of the model, i.e. size of joint vectors

enum iDynTree::InverseKinematicsRotationParametrization internal::kinematics::InverseKinematicsData::m_rotationParametrization

type of parametrization of the orientation

TransformMap internal::kinematics::InverseKinematicsData::m_constraints

list of hard constraints

TransformMap internal::kinematics::InverseKinematicsData::m_targets

list of targets

iDynTree::ConvexHullProjectionConstraint internal::kinematics::InverseKinematicsData::m_comHullConstraint

Helper to implement COM constraint

bool internal::kinematics::InverseKinematicsData::m_areBaseInitialConditionsSet

True if initial condition for the base pose are provided by the user

InverseKinematicsInitialConditionType internal::kinematics::InverseKinematicsData::m_areJointsInitialConditionsSet

specify if the initial condition for the joints are provided by the user

int internal::kinematics::InverseKinematicsData::m_maxIter

Maximum number of iterations

double internal::kinematics::InverseKinematicsData::m_maxCpuTime

Maximum CPU time

double internal::kinematics::InverseKinematicsData::m_tol

Tolerance for the cost

double internal::kinematics::InverseKinematicsData::m_constrTol

Tolerance for the constraints

int internal::kinematics::InverseKinematicsData::m_verbosityLevel

Verbosity level