- 3.0.2 rigid body dynamics module.
ct::rbd::TermTaskspaceGeometricJacobian< KINEMATICS, STATE_DIM, CONTROL_DIM > Class Template Reference

A costfunction term that defines a cost on a task-space pose, for fix-base robots only. More...

#include <TermTaskspaceGeometricJacobian.hpp>

Inheritance diagram for ct::rbd::TermTaskspaceGeometricJacobian< KINEMATICS, STATE_DIM, CONTROL_DIM >:
ct::optcon::TermBase< STATE_DIM, CONTROL_DIM, double, double >

Public Types

using BASE = optcon::TermBase< STATE_DIM, CONTROL_DIM, double, double >
 
using RBDState = ct::rbd::RBDState< KINEMATICS::NJOINTS >
 
using state_matrix_t = Eigen::Matrix< double, STATE_DIM, STATE_DIM >
 
using control_matrix_t = Eigen::Matrix< double, CONTROL_DIM, CONTROL_DIM >
 
using control_state_matrix_t = Eigen::Matrix< double, CONTROL_DIM, STATE_DIM >
 
- Public Types inherited from ct::optcon::TermBase< STATE_DIM, CONTROL_DIM, double, double >
typedef Eigen::Matrix< double, STATE_DIM, STATE_DIM > state_matrix_t
 
typedef Eigen::Matrix< double, CONTROL_DIM, CONTROL_DIM > control_matrix_t
 
typedef Eigen::Matrix< double, CONTROL_DIM, STATE_DIM > control_state_matrix_t
 
typedef Eigen::Matrix< double, STATE_DIM, STATE_DIM > state_matrix_double_t
 
typedef Eigen::Matrix< double, CONTROL_DIM, CONTROL_DIM > control_matrix_double_t
 
typedef Eigen::Matrix< double, CONTROL_DIM, STATE_DIM > control_state_matrix_double_t
 

Public Member Functions

 TermTaskspaceGeometricJacobian ()=default
 default constructor More...
 
 TermTaskspaceGeometricJacobian (size_t eeInd, const Eigen::Matrix3d &Qpos, const Eigen::Matrix3d &Qrot, const core::StateVector< 3 > &w_pos_des, const Eigen::Quaterniond &w_q_des, const std::string &name="TermTaskSpace")
 constructor using a quaternion for orientation More...
 
 TermTaskspaceGeometricJacobian (size_t eeInd, const Eigen::Matrix3d &Qpos, const Eigen::Matrix3d &Qrot, const ct::rbd::RigidBodyPose &rbdPose, const std::string &name="TermTaskSpace")
 constructor taking a full RigidBodyPose More...
 
 TermTaskspaceGeometricJacobian (size_t eeInd, const Eigen::Matrix3d &Qpos, const Eigen::Matrix3d &Qrot, const core::StateVector< 3 > &w_pos_des, const Eigen::Matrix3d &eulerXyz, const std::string &name="TermTaskSpace")
 constructor using Euler angles for orientation More...
 
 TermTaskspaceGeometricJacobian (size_t eeInd, const Eigen::Matrix3d &Qpos, const Eigen::Matrix3d &Qrot, const std::string &name="TermTaskSpace")
 constructor which sets the target pose to dummy values More...
 
 TermTaskspaceGeometricJacobian (const std::string &configFile, const std::string &termName, bool verbose=false)
 construct this term with info loaded from a configuration file More...
 
 TermTaskspaceGeometricJacobian (const TermTaskspaceGeometricJacobian &arg)
 copy constructor More...
 
virtual ~TermTaskspaceGeometricJacobian ()=default
 destructor More...
 
virtual TermTaskspaceGeometricJacobian< KINEMATICS, STATE_DIM, CONTROL_DIM > * clone () const override
 deep cloning More...
 
virtual double evaluate (const Eigen::Matrix< double, STATE_DIM, 1 > &x, const Eigen::Matrix< double, CONTROL_DIM, 1 > &u, const double &t) override
 evaluate More...
 
virtual core::StateVector< STATE_DIM > stateDerivative (const core::StateVector< STATE_DIM > &x, const core::ControlVector< CONTROL_DIM > &u, const double &t) override
 compute derivative of this cost term w.r.t. the state More...
 
virtual core::ControlVector< CONTROL_DIM > controlDerivative (const core::StateVector< STATE_DIM > &x, const core::ControlVector< CONTROL_DIM > &u, const double &t) override
 compute derivative of this cost term w.r.t. the control input More...
 
virtual state_matrix_t stateSecondDerivative (const core::StateVector< STATE_DIM > &x, const core::ControlVector< CONTROL_DIM > &u, const double &t) override
 compute second order derivative of this cost term w.r.t. the state More...
 
virtual control_matrix_t controlSecondDerivative (const core::StateVector< STATE_DIM > &x, const core::ControlVector< CONTROL_DIM > &u, const double &t) override
 compute second order derivative of this cost term w.r.t. the control input More...
 
virtual control_state_matrix_t stateControlDerivative (const core::StateVector< STATE_DIM > &x, const core::ControlVector< CONTROL_DIM > &u, const double &t) override
 compute the cross-term derivative (state-control) of this cost function term More...
 
void loadConfigFile (const std::string &filename, const std::string &termName, bool verbose=false) override
 load term information from configuration file (stores data in member variables) More...
 
const Eigen::Matrix< double, 3, 1 > getReferencePosition () const
 retrieve reference position in world frame More...
 
void setReferencePosition (Eigen::Matrix< double, 3, 1 > p_ref)
 set the end-effector reference position (in base coordinates) More...
 
const Eigen::Quaterniond getReferenceOrientation () const
 retrieve reference ee orientation in world frame More...
 
void setReferenceOrientation (const Eigen::Matrix< double, 3, 3 > &w_R_ref)
 set desired end-effector orientation in world frame More...
 
void setReferenceOrientation (const Eigen::Quaterniond &w_q_des)
 set desired end-effector orientation in world frame More...
 
const ct::rbd::RigidBodyPose getReferencePose () const
 return the reference pose as RigidBodyPose More...
 
- Public Member Functions inherited from ct::optcon::TermBase< STATE_DIM, CONTROL_DIM, double, double >
 TermBase (std::string name="Unnamed")
 
 TermBase (const TermBase &arg)
 
virtual ~TermBase ()
 
double eval (const Eigen::Matrix< double, STATE_DIM, 1 > &x, const Eigen::Matrix< double, CONTROL_DIM, 1 > &u, const double &t)
 
virtual bool isActiveAtTime (double t)
 
double computeActivation (double t)
 
virtual core::StateVector< STATE_DIM, double > stateDerivative (const core::StateVector< STATE_DIM, double > &x, const core::ControlVector< CONTROL_DIM, double > &u, const double &t)
 
virtual state_matrix_t stateSecondDerivative (const core::StateVector< STATE_DIM, double > &x, const core::ControlVector< CONTROL_DIM, double > &u, const double &t)
 
virtual core::ControlVector< CONTROL_DIM, double > controlDerivative (const core::StateVector< STATE_DIM, double > &x, const core::ControlVector< CONTROL_DIM, double > &u, const double &t)
 
virtual control_matrix_t controlSecondDerivative (const core::StateVector< STATE_DIM, double > &x, const core::ControlVector< CONTROL_DIM, double > &u, const double &t)
 
virtual control_state_matrix_t stateControlDerivative (const core::StateVector< STATE_DIM, double > &x, const core::ControlVector< CONTROL_DIM, double > &u, const double &t)
 
void setTimeActivation (std::shared_ptr< ct::core::tpl::ActivationBase< double >> c_i, bool verbose=false)
 
void loadTimeActivation (const std::string &filename, const std::string &termName, bool verbose=false)
 
const std::string & getName () const
 
void setName (const std::string &termName)
 
virtual void updateReferenceState (const Eigen::Matrix< double, STATE_DIM, 1 > &newRefState)
 
virtual void updateReferenceControl (const Eigen::Matrix< double, CONTROL_DIM, 1 > &newRefControl)
 
virtual Eigen::Matrix< double, STATE_DIM, 1 > getReferenceState () const
 

Additional Inherited Members

- Protected Attributes inherited from ct::optcon::TermBase< STATE_DIM, CONTROL_DIM, double, double >
std::string name_
 
std::shared_ptr< ct::core::tpl::ActivationBase< double > > c_i_
 

Detailed Description

template<class KINEMATICS, size_t STATE_DIM, size_t CONTROL_DIM>
class ct::rbd::TermTaskspaceGeometricJacobian< KINEMATICS, STATE_DIM, CONTROL_DIM >

A costfunction term that defines a cost on a task-space pose, for fix-base robots only.

Template Parameters
KINEMATICSkinematics of the system
STATE_DIMstate dimensionality of the system
CONTROL_DIMcontrol dimensionality of the system

Member Typedef Documentation

◆ BASE

template<class KINEMATICS , size_t STATE_DIM, size_t CONTROL_DIM>
using ct::rbd::TermTaskspaceGeometricJacobian< KINEMATICS, STATE_DIM, CONTROL_DIM >::BASE = optcon::TermBase<STATE_DIM, CONTROL_DIM, double, double>

◆ RBDState

template<class KINEMATICS , size_t STATE_DIM, size_t CONTROL_DIM>
using ct::rbd::TermTaskspaceGeometricJacobian< KINEMATICS, STATE_DIM, CONTROL_DIM >::RBDState = ct::rbd::RBDState<KINEMATICS::NJOINTS>

◆ state_matrix_t

template<class KINEMATICS , size_t STATE_DIM, size_t CONTROL_DIM>
using ct::rbd::TermTaskspaceGeometricJacobian< KINEMATICS, STATE_DIM, CONTROL_DIM >::state_matrix_t = Eigen::Matrix<double, STATE_DIM, STATE_DIM>

◆ control_matrix_t

template<class KINEMATICS , size_t STATE_DIM, size_t CONTROL_DIM>
using ct::rbd::TermTaskspaceGeometricJacobian< KINEMATICS, STATE_DIM, CONTROL_DIM >::control_matrix_t = Eigen::Matrix<double, CONTROL_DIM, CONTROL_DIM>

◆ control_state_matrix_t

template<class KINEMATICS , size_t STATE_DIM, size_t CONTROL_DIM>
using ct::rbd::TermTaskspaceGeometricJacobian< KINEMATICS, STATE_DIM, CONTROL_DIM >::control_state_matrix_t = Eigen::Matrix<double, CONTROL_DIM, STATE_DIM>

Constructor & Destructor Documentation

◆ TermTaskspaceGeometricJacobian() [1/7]

template<class KINEMATICS , size_t STATE_DIM, size_t CONTROL_DIM>
ct::rbd::TermTaskspaceGeometricJacobian< KINEMATICS, STATE_DIM, CONTROL_DIM >::TermTaskspaceGeometricJacobian ( )
default

◆ TermTaskspaceGeometricJacobian() [2/7]

template<class KINEMATICS , size_t STATE_DIM, size_t CONTROL_DIM>
ct::rbd::TermTaskspaceGeometricJacobian< KINEMATICS, STATE_DIM, CONTROL_DIM >::TermTaskspaceGeometricJacobian ( size_t  eeInd,
const Eigen::Matrix3d &  Qpos,
const Eigen::Matrix3d &  Qrot,
const core::StateVector< 3 > &  w_pos_des,
const Eigen::Quaterniond &  w_q_des,
const std::string &  name = "TermTaskSpace" 
)
inline

◆ TermTaskspaceGeometricJacobian() [3/7]

template<class KINEMATICS , size_t STATE_DIM, size_t CONTROL_DIM>
ct::rbd::TermTaskspaceGeometricJacobian< KINEMATICS, STATE_DIM, CONTROL_DIM >::TermTaskspaceGeometricJacobian ( size_t  eeInd,
const Eigen::Matrix3d &  Qpos,
const Eigen::Matrix3d &  Qrot,
const ct::rbd::RigidBodyPose rbdPose,
const std::string &  name = "TermTaskSpace" 
)
inline

constructor taking a full RigidBodyPose

◆ TermTaskspaceGeometricJacobian() [4/7]

template<class KINEMATICS , size_t STATE_DIM, size_t CONTROL_DIM>
ct::rbd::TermTaskspaceGeometricJacobian< KINEMATICS, STATE_DIM, CONTROL_DIM >::TermTaskspaceGeometricJacobian ( size_t  eeInd,
const Eigen::Matrix3d &  Qpos,
const Eigen::Matrix3d &  Qrot,
const core::StateVector< 3 > &  w_pos_des,
const Eigen::Matrix3d &  eulerXyz,
const std::string &  name = "TermTaskSpace" 
)
inline

constructor using Euler angles for orientation

◆ TermTaskspaceGeometricJacobian() [5/7]

template<class KINEMATICS , size_t STATE_DIM, size_t CONTROL_DIM>
ct::rbd::TermTaskspaceGeometricJacobian< KINEMATICS, STATE_DIM, CONTROL_DIM >::TermTaskspaceGeometricJacobian ( size_t  eeInd,
const Eigen::Matrix3d &  Qpos,
const Eigen::Matrix3d &  Qrot,
const std::string &  name = "TermTaskSpace" 
)
inline

◆ TermTaskspaceGeometricJacobian() [6/7]

template<class KINEMATICS , size_t STATE_DIM, size_t CONTROL_DIM>
ct::rbd::TermTaskspaceGeometricJacobian< KINEMATICS, STATE_DIM, CONTROL_DIM >::TermTaskspaceGeometricJacobian ( const std::string &  configFile,
const std::string &  termName,
bool  verbose = false 
)
inline

construct this term with info loaded from a configuration file

References ct::rbd::TermTaskspaceGeometricJacobian< KINEMATICS, STATE_DIM, CONTROL_DIM >::loadConfigFile(), and verbose.

◆ TermTaskspaceGeometricJacobian() [7/7]

template<class KINEMATICS , size_t STATE_DIM, size_t CONTROL_DIM>
ct::rbd::TermTaskspaceGeometricJacobian< KINEMATICS, STATE_DIM, CONTROL_DIM >::TermTaskspaceGeometricJacobian ( const TermTaskspaceGeometricJacobian< KINEMATICS, STATE_DIM, CONTROL_DIM > &  arg)
inline

◆ ~TermTaskspaceGeometricJacobian()

template<class KINEMATICS , size_t STATE_DIM, size_t CONTROL_DIM>
virtual ct::rbd::TermTaskspaceGeometricJacobian< KINEMATICS, STATE_DIM, CONTROL_DIM >::~TermTaskspaceGeometricJacobian ( )
virtualdefault

Member Function Documentation

◆ clone()

template<class KINEMATICS , size_t STATE_DIM, size_t CONTROL_DIM>
virtual TermTaskspaceGeometricJacobian<KINEMATICS, STATE_DIM, CONTROL_DIM>* ct::rbd::TermTaskspaceGeometricJacobian< KINEMATICS, STATE_DIM, CONTROL_DIM >::clone ( ) const
inlineoverridevirtual

◆ evaluate()

template<class KINEMATICS , size_t STATE_DIM, size_t CONTROL_DIM>
virtual double ct::rbd::TermTaskspaceGeometricJacobian< KINEMATICS, STATE_DIM, CONTROL_DIM >::evaluate ( const Eigen::Matrix< double, STATE_DIM, 1 > &  x,
const Eigen::Matrix< double, CONTROL_DIM, 1 > &  u,
const double &  t 
)
inlineoverridevirtual

◆ stateDerivative()

template<class KINEMATICS , size_t STATE_DIM, size_t CONTROL_DIM>
virtual core::StateVector<STATE_DIM> ct::rbd::TermTaskspaceGeometricJacobian< KINEMATICS, STATE_DIM, CONTROL_DIM >::stateDerivative ( const core::StateVector< STATE_DIM > &  x,
const core::ControlVector< CONTROL_DIM > &  u,
const double &  t 
)
inlineoverridevirtual

compute derivative of this cost term w.r.t. the state

References ct::rbd::RBDState< NJOINTS, SCALAR >::basePose(), and ct::rbd::RBDState< NJOINTS, SCALAR >::jointPositions().

◆ controlDerivative()

template<class KINEMATICS , size_t STATE_DIM, size_t CONTROL_DIM>
virtual core::ControlVector<CONTROL_DIM> ct::rbd::TermTaskspaceGeometricJacobian< KINEMATICS, STATE_DIM, CONTROL_DIM >::controlDerivative ( const core::StateVector< STATE_DIM > &  x,
const core::ControlVector< CONTROL_DIM > &  u,
const double &  t 
)
inlineoverridevirtual

compute derivative of this cost term w.r.t. the control input

◆ stateSecondDerivative()

template<class KINEMATICS , size_t STATE_DIM, size_t CONTROL_DIM>
virtual state_matrix_t ct::rbd::TermTaskspaceGeometricJacobian< KINEMATICS, STATE_DIM, CONTROL_DIM >::stateSecondDerivative ( const core::StateVector< STATE_DIM > &  x,
const core::ControlVector< CONTROL_DIM > &  u,
const double &  t 
)
inlineoverridevirtual

compute second order derivative of this cost term w.r.t. the state

References ct::rbd::RBDState< NJOINTS, SCALAR >::jointPositions().

◆ controlSecondDerivative()

template<class KINEMATICS , size_t STATE_DIM, size_t CONTROL_DIM>
virtual control_matrix_t ct::rbd::TermTaskspaceGeometricJacobian< KINEMATICS, STATE_DIM, CONTROL_DIM >::controlSecondDerivative ( const core::StateVector< STATE_DIM > &  x,
const core::ControlVector< CONTROL_DIM > &  u,
const double &  t 
)
inlineoverridevirtual

compute second order derivative of this cost term w.r.t. the control input

◆ stateControlDerivative()

template<class KINEMATICS , size_t STATE_DIM, size_t CONTROL_DIM>
virtual control_state_matrix_t ct::rbd::TermTaskspaceGeometricJacobian< KINEMATICS, STATE_DIM, CONTROL_DIM >::stateControlDerivative ( const core::StateVector< STATE_DIM > &  x,
const core::ControlVector< CONTROL_DIM > &  u,
const double &  t 
)
inlineoverridevirtual

compute the cross-term derivative (state-control) of this cost function term

◆ loadConfigFile()

template<class KINEMATICS , size_t STATE_DIM, size_t CONTROL_DIM>
void ct::rbd::TermTaskspaceGeometricJacobian< KINEMATICS, STATE_DIM, CONTROL_DIM >::loadConfigFile ( const std::string &  filename,
const std::string &  termName,
bool  verbose = false 
)
inlineoverridevirtual

◆ getReferencePosition()

template<class KINEMATICS , size_t STATE_DIM, size_t CONTROL_DIM>
const Eigen::Matrix<double, 3, 1> ct::rbd::TermTaskspaceGeometricJacobian< KINEMATICS, STATE_DIM, CONTROL_DIM >::getReferencePosition ( ) const
inline

◆ setReferencePosition()

template<class KINEMATICS , size_t STATE_DIM, size_t CONTROL_DIM>
void ct::rbd::TermTaskspaceGeometricJacobian< KINEMATICS, STATE_DIM, CONTROL_DIM >::setReferencePosition ( Eigen::Matrix< double, 3, 1 >  p_ref)
inline

◆ getReferenceOrientation()

template<class KINEMATICS , size_t STATE_DIM, size_t CONTROL_DIM>
const Eigen::Quaterniond ct::rbd::TermTaskspaceGeometricJacobian< KINEMATICS, STATE_DIM, CONTROL_DIM >::getReferenceOrientation ( ) const
inline

retrieve reference ee orientation in world frame

Referenced by ct::rbd::TermTaskspaceGeometricJacobian< KINEMATICS, STATE_DIM, CONTROL_DIM >::getReferencePose().

◆ setReferenceOrientation() [1/2]

template<class KINEMATICS , size_t STATE_DIM, size_t CONTROL_DIM>
void ct::rbd::TermTaskspaceGeometricJacobian< KINEMATICS, STATE_DIM, CONTROL_DIM >::setReferenceOrientation ( const Eigen::Matrix< double, 3, 3 > &  w_R_ref)
inline

◆ setReferenceOrientation() [2/2]

template<class KINEMATICS , size_t STATE_DIM, size_t CONTROL_DIM>
void ct::rbd::TermTaskspaceGeometricJacobian< KINEMATICS, STATE_DIM, CONTROL_DIM >::setReferenceOrientation ( const Eigen::Quaterniond &  w_q_des)
inline

◆ getReferencePose()

template<class KINEMATICS , size_t STATE_DIM, size_t CONTROL_DIM>
const ct::rbd::RigidBodyPose ct::rbd::TermTaskspaceGeometricJacobian< KINEMATICS, STATE_DIM, CONTROL_DIM >::getReferencePose ( ) const
inline

The documentation for this class was generated from the following file: