A costfunction term that defines a cost on a task-space pose, for fix-base robots only.
More...
|
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 > |
|
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 |
|
|
| 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...
|
|
| 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 |
|
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
-
KINEMATICS | kinematics of the system |
STATE_DIM | state dimensionality of the system |
CONTROL_DIM | control dimensionality of the system |