Robot Dynamics Library
Kinematics.hpp
Go to the documentation of this file.
1 // Copyright (c) 2017 Jordan Lack <jlack1987@gmail.com>
2 // Copyright (c) 2011-2016 Martin Felis <martin.felis@iwr.uni-heidelberg.de>
3 // RDL - Robot Dynamics Library
4 // Licensed under the zlib license. See LICENSE for more details.
5 
6 #ifndef RDL_KINEMATICS_H
7 #define RDL_KINEMATICS_H
8 
13 #include <assert.h>
14 #include <iostream>
15 #include <rdl_dynamics/FrameVectorPair.hpp>
16 #include <rdl_dynamics/Model.hpp>
17 
18 namespace RobotDynamics
19 {
48 void updateKinematics(Model& model, const Math::VectorNd& Q, const Math::VectorNd& QDot, const Math::VectorNd& QDDot);
49 
67 void updateKinematicsCustom(Model& model, const Math::VectorNd* Q, const Math::VectorNd* QDot = nullptr, const Math::VectorNd* QDDot = nullptr);
68 
90 void updateKinematicsParallel(Model& model, const Math::VectorNd& Q, const Math::VectorNd& QDot, const Math::VectorNd& QDDot);
91 
120 void updateKinematicsCustomParallel(Model& model, const Math::VectorNd* Q, const Math::VectorNd* QDot = nullptr, const Math::VectorNd* QDDot = nullptr);
121 
129 void updateAccelerations(Model& model, const Math::VectorNd& QDDot);
130 
151 void calcPointJacobian(Model& model, const Math::VectorNd& Q, unsigned int body_id, const Math::Vector3d& point_position, Math::MatrixNd& G,
152  bool update_kinematics = true);
153 
169 void calcPointJacobian(Model& model, const Math::VectorNd& Q, Math::MatrixNd& G, ReferenceFramePtr frame, bool update_kinematics = true);
170 
187 void calcPointJacobian6D(Model& model, const Math::VectorNd& Q, Math::MatrixNd& G, ReferenceFramePtr frame, bool update_kinematics = true);
188 
204 void calcRelativePointJacobian6D(Model& model, const Math::VectorNd& Q, Math::MatrixNd& G, ReferenceFramePtr baseFrame, ReferenceFramePtr relativeFrame,
205  ReferenceFramePtr expressedInFrame = RobotDynamics::ReferenceFrame::getWorldFrame(), bool update_kinematics = true);
222 void calcRelativePointJacobianDot6D(Model& model, const Math::VectorNd& Q, const Math::VectorNd& QDot, Math::MatrixNd& G, ReferenceFramePtr baseFrame,
223  ReferenceFramePtr relativeFrame, ReferenceFramePtr expressedInFrame = RobotDynamics::ReferenceFrame::getWorldFrame(),
224  bool update_kinematics = true);
225 
249 void calcRelativePointJacobianAndJacobianDot6D(Model& model, const Math::VectorNd& Q, const Math::VectorNd& QDot, Math::MatrixNd& G, Math::MatrixNd& GDot,
250  ReferenceFramePtr baseFrame, ReferenceFramePtr relativeFrame,
251  ReferenceFramePtr expressedInFrame = RobotDynamics::ReferenceFrame::getWorldFrame(), bool update_kinematics = true);
252 
274 void calcRelativeBodySpatialJacobian(Model& model, const Math::VectorNd& Q, Math::MatrixNd& G, ReferenceFramePtr baseFrame, ReferenceFramePtr relativeFrame,
275  ReferenceFramePtr expressedInFrame = nullptr, bool update_kinematics = true);
276 
295 void calcRelativeBodySpatialJacobianDot(Model& model, const Math::VectorNd& Q, const Math::VectorNd& QDot, Math::MatrixNd& G, ReferenceFramePtr baseFrame,
296  ReferenceFramePtr relativeFrame, ReferenceFramePtr expressedInFrame = nullptr, bool update_kinematics = true);
297 
313 void calcRelativeBodySpatialJacobianAndJacobianDot(Model& model, const Math::VectorNd& Q, const Math::VectorNd& QDot, Math::MatrixNd& G, Math::MatrixNd& GDot,
314  ReferenceFramePtr baseFrame, ReferenceFramePtr relativeFrame, ReferenceFramePtr expressedInFrame = nullptr,
315  bool update_kinematics = true);
316 
338 void calcPointJacobianDot(Model& model, const Math::VectorNd& Q, const Math::VectorNd& QDot, unsigned int body_id, const Math::Vector3d& point_position,
339  Math::MatrixNd& G, bool update_kinematics = true);
340 
361 void calcPointJacobianDot6D(Model& model, const Math::VectorNd& Q, const Math::VectorNd& QDot, RobotDynamics::ReferenceFramePtr frame, Math::MatrixNd& G,
362  bool update_kinematics = true);
363 
384 void calcPointJacobianDot(Model& model, const Math::VectorNd& Q, const Math::VectorNd& QDot, RobotDynamics::ReferenceFramePtr frame, Math::MatrixNd& G,
385  bool update_kinematics = true);
386 
407 void calcPointJacobian6D(Model& model, const Math::VectorNd& Q, unsigned int body_id, const Math::Vector3d& point_position, Math::MatrixNd& G,
408  bool update_kinematics = true);
409 
434 void calcPointJacobianDot6D(Model& model, const Math::VectorNd& Q, const Math::VectorNd& QDot, unsigned int body_id, const Math::Vector3d& point_position,
435  Math::MatrixNd& G, bool update_kinematics = true);
436 
466 void calcBodySpatialJacobian(Model& model, const Math::VectorNd& Q, unsigned int body_id, Math::MatrixNd& G, bool update_kinematics = true);
467 
487 void calcBodySpatialJacobianDot(Model& model, const Math::VectorNd& Q, const Math::VectorNd QDot, unsigned int body_id, Math::MatrixNd& G,
488  const bool update_kinematics = true);
489 
501 Math::FrameVector calcPointVelocity(Model& model, const Math::VectorNd& Q, const Math::VectorNd& QDot, unsigned int body_id, const Math::Vector3d& point_position,
502  bool update_kinematics = true);
503 
515 Math::FrameVectorPair calcPointVelocity6D(Model& model, const Math::VectorNd& Q, const Math::VectorNd& QDot, unsigned int body_id, const Math::Vector3d& point_position,
516  bool update_kinematics = true);
517 
529 Math::FrameVectorPair calcPointVelocity6D(Model& model, const Math::VectorNd& Q, const Math::VectorNd& QDot, RobotDynamics::ReferenceFramePtr frame,
530  bool update_kinematics = true);
531 
547 Math::FrameVectorPair calcPointVelocity6D(Model& model, const Math::VectorNd& Q, const Math::VectorNd& QDot, RobotDynamics::ReferenceFramePtr baseFrame,
548  RobotDynamics::ReferenceFramePtr relativeFrame,
549  RobotDynamics::ReferenceFramePtr expressedInFrame = RobotDynamics::ReferenceFrame::getWorldFrame(),
550  bool update_kinematics = true);
551 
574 Math::FrameVector calcPointAcceleration(Model& model, const Math::VectorNd& Q, const Math::VectorNd& QDot, const Math::VectorNd& QDDot, unsigned int body_id,
575  const Math::Vector3d& point_position, bool update_kinematics = true);
576 
600 Math::SpatialMotion calcSpatialVelocity(Model& model, const Math::VectorNd& Q, const Math::VectorNd& QDot, ReferenceFramePtr body_frame,
601  ReferenceFramePtr relative_body_frame, ReferenceFramePtr expressedInFrame = nullptr, const bool update_kinematics = true);
602 
626 Math::SpatialMotion calcSpatialVelocity(Model& model, const Math::VectorNd& Q, const Math::VectorNd& QDot, const unsigned int body_id,
627  const unsigned int relative_body_id, ReferenceFramePtr expressedInFrame = nullptr, const bool update_kinematics = true);
628 
650 Math::SpatialAcceleration calcSpatialAcceleration(Model& model, const Math::VectorNd& Q, const Math::VectorNd& QDot, const Math::VectorNd& QDDot,
651  const unsigned int body_id, const unsigned int relative_body_id, ReferenceFramePtr expressedInFrame = nullptr,
652  const bool update_kinematics = true);
653 
675 Math::SpatialAcceleration calcSpatialAcceleration(Model& model, const Math::VectorNd& Q, const Math::VectorNd& QDot, const Math::VectorNd& QDDot,
676  ReferenceFramePtr body_frame, ReferenceFramePtr relative_body_frame, ReferenceFramePtr expressedInFrame = nullptr,
677  const bool update_kinematics = true);
678 
700 Math::FrameVectorPair calcPointAcceleration6D(Model& model, const Math::VectorNd& Q, const Math::VectorNd& QDot, const Math::VectorNd& QDDot, unsigned int body_id,
701  const Math::Vector3d& point_position, bool update_kinematics = true);
702 
727 Math::FrameVectorPair calcPointAcceleration6D(Model& model, const Math::VectorNd& Q, const Math::VectorNd& QDot, const Math::VectorNd& QDDot,
728  ReferenceFramePtr body_frame, ReferenceFramePtr relative_body_frame, ReferenceFramePtr expressedInFrame = nullptr,
729  const bool update_kinematics = true);
730 
763 bool inverseKinematics(Model& model, const Math::VectorNd& Qinit, const std::vector<unsigned int>& body_id, const std::vector<Math::Vector3d>& body_point,
764  const std::vector<Math::Vector3d>& target_pos, Math::VectorNd& Qres, double step_tol = 1.0e-12, double lambda = 0.01, unsigned int max_iter = 50);
765 
767 } // namespace RobotDynamics
768 
769 /* RDL_KINEMATICS_H */
770 #endif // ifndef RDL_KINEMATICS_H
RobotDynamics::calcPointJacobian
void calcPointJacobian(Model &model, const VectorNd &Q, unsigned int body_id, const Vector3d &point_position, MatrixNd &G, bool update_kinematics)
Computes the 3D point jacobian for a point on a body.
Definition: Kinematics.cpp:526
RobotDynamics::updateKinematicsParallel
void updateKinematicsParallel(Model &model, const VectorNd &Q, const VectorNd &QDot, const VectorNd &QDDot)
Updates and computes velocities and accelerations of the bodies.
Definition: Kinematics.cpp:205
RobotDynamics::calcRelativeBodySpatialJacobianAndJacobianDot
void calcRelativeBodySpatialJacobianAndJacobianDot(Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDot, Math::MatrixNd &G, Math::MatrixNd &GDot, ReferenceFramePtr baseFrame, ReferenceFramePtr relativeFrame, ReferenceFramePtr expressedInFrame, bool update_kinematics)
Computes both the body spatial jacobian and its time derivative. This function will be a bit more eff...
Definition: Kinematics.cpp:405
RobotDynamics::calcPointJacobianDot
void calcPointJacobianDot(Model &model, const VectorNd &Q, const VectorNd &QDot, unsigned int body_id, const Vector3d &point_position, MatrixNd &G, bool update_kinematics)
Computes the time derivative of the linear components the a point jacobian on a body.
Definition: Kinematics.cpp:767
RobotDynamics::calcRelativePointJacobianAndJacobianDot6D
void calcRelativePointJacobianAndJacobianDot6D(Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDot, Math::MatrixNd &G, Math::MatrixNd &GDot, ReferenceFramePtr baseFrame, ReferenceFramePtr relativeFrame, ReferenceFramePtr expressedInFrame, bool update_kinematics)
Compute the point Jacobian of the origin of baseFrame, relative to the origin of relativeFrame,...
Definition: Kinematics.cpp:1062
RobotDynamics::updateKinematicsCustom
void updateKinematicsCustom(Model &model, const VectorNd *Q, const VectorNd *QDot, const VectorNd *QDDot)
Selectively updates model internal states of body positions, velocities and/or accelerations.
Definition: Kinematics.cpp:66
RobotDynamics::calcPointVelocity6D
Math::FrameVectorPair calcPointVelocity6D(Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDot, unsigned int body_id, const Math::Vector3d &point_position, bool update_kinematics)
Computes angular and linear velocity of a point that is fixed on a body.
Definition: Kinematics.cpp:1509
RobotDynamics::calcRelativePointJacobianDot6D
void calcRelativePointJacobianDot6D(Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDot, Math::MatrixNd &G, ReferenceFramePtr baseFrame, ReferenceFramePtr relativeFrame, ReferenceFramePtr expressedInFrame, bool update_kinematics)
Compute the time derivative of the 6D jacobian of the origin of a reference frame relative to the ori...
Definition: Kinematics.cpp:896
RobotDynamics::calcBodySpatialJacobianDot
void calcBodySpatialJacobianDot(Model &model, const Math::VectorNd &Q, const Math::VectorNd QDot, unsigned int body_id, Math::MatrixNd &G, const bool update_kinematics)
Computes the time derivative of the spatial jacobian for a body. The result will be returned via the ...
Definition: Kinematics.cpp:1415
RobotDynamics::calcRelativePointJacobian6D
void calcRelativePointJacobian6D(Model &model, const Math::VectorNd &Q, Math::MatrixNd &G, ReferenceFramePtr baseFrame, ReferenceFramePtr relativeFrame, ReferenceFramePtr expressedInFrame, bool update_kinematics)
Compute the 6D jacobian of the origin of a reference frame relative to the origin of another referenc...
Definition: Kinematics.cpp:661
RobotDynamics::calcBodySpatialJacobian
void calcBodySpatialJacobian(Model &model, const VectorNd &Q, unsigned int body_id, MatrixNd &G, bool update_kinematics)
Computes the spatial jacobian for a body. The result will be returned via the G argument and represen...
Definition: Kinematics.cpp:1357
RobotDynamics::calcSpatialAcceleration
Math::SpatialAcceleration calcSpatialAcceleration(Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDot, const Math::VectorNd &QDDot, ReferenceFramePtr body_frame, ReferenceFramePtr relative_body_frame, ReferenceFramePtr expressedInFrame, const bool update_kinematics)
Compute the spatial acceleration of any frame with respect to any other frame and express the result ...
Definition: Kinematics.cpp:1818
RobotDynamics::updateKinematicsCustomParallel
void updateKinematicsCustomParallel(Model &model, const VectorNd *Q, const VectorNd *QDot, const VectorNd *QDDot)
Selectively updates model internal states of body positions, velocities and/or accelerations and spaw...
Definition: Kinematics.cpp:187
RobotDynamics::calcPointJacobianDot6D
void calcPointJacobianDot6D(Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDot, RobotDynamics::ReferenceFramePtr frame, Math::MatrixNd &G, bool update_kinematics)
Computes the time derivative of a point jacobian of a point on a body.
Definition: Kinematics.cpp:842
RobotDynamics::updateAccelerations
void updateAccelerations(Model &model, const VectorNd &QDDot)
Computes only the accelerations of the bodies.
Definition: Kinematics.cpp:145
RobotDynamics::calcRelativeBodySpatialJacobian
void calcRelativeBodySpatialJacobian(Model &model, const Math::VectorNd &Q, Math::MatrixNd &G, ReferenceFramePtr baseFrame, ReferenceFramePtr relativeFrame, ReferenceFramePtr expressedInFrame, bool update_kinematics)
Computes the jacobian of a frame with being the "base" frame, being the "relative" frame,...
Definition: Kinematics.cpp:223
RobotDynamics::calcPointAcceleration
Math::FrameVector calcPointAcceleration(Model &model, const VectorNd &Q, const VectorNd &QDot, const VectorNd &QDDot, unsigned int body_id, const Vector3d &point_position, bool update_kinematics)
Computes the linear acceleration of a point on a body.
Definition: Kinematics.cpp:1634
RobotDynamics::calcPointVelocity
Math::FrameVector calcPointVelocity(Model &model, const VectorNd &Q, const VectorNd &QDot, unsigned int body_id, const Vector3d &point_position, bool update_kinematics)
Computes the velocity of a point on a body.
Definition: Kinematics.cpp:1473
RobotDynamics::calcPointJacobian6D
void calcPointJacobian6D(Model &model, const Math::VectorNd &Q, Math::MatrixNd &G, ReferenceFramePtr frame, bool update_kinematics)
Compute the 6D point jacobian of the origin of a reference frame If a position of a point is computed...
Definition: Kinematics.cpp:622
RobotDynamics::calcPointAcceleration6D
FrameVectorPair calcPointAcceleration6D(Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDot, const Math::VectorNd &QDDot, ReferenceFramePtr body_frame, ReferenceFramePtr relative_body_frame, ReferenceFramePtr expressedInFrame, const bool update_kinematics)
Computes linear and angular acceleration of a reference frame, relative to another reference frame an...
Definition: Kinematics.cpp:1590
RobotDynamics::calcSpatialVelocity
Math::SpatialMotion calcSpatialVelocity(Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDot, ReferenceFramePtr body_frame, ReferenceFramePtr relative_body_frame, ReferenceFramePtr expressedInFrame, const bool update_kinematics)
Compute the spatial velocity of any frame with respect to any other frame, expressed in an arbirtary ...
Definition: Kinematics.cpp:1710
RobotDynamics::updateKinematics
void updateKinematics(Model &model, const VectorNd &Q, const VectorNd &QDot, const VectorNd &QDDot)
Updates and computes velocities and accelerations of the bodies.
Definition: Kinematics.cpp:19
RobotDynamics::calcRelativeBodySpatialJacobianDot
void calcRelativeBodySpatialJacobianDot(Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDot, Math::MatrixNd &G, ReferenceFramePtr baseFrame, ReferenceFramePtr relativeFrame, ReferenceFramePtr expressedInFrame, bool update_kinematics)
Computes the rate of change of the jacobian, , with being the "base" frame, being the relative" fra...
Definition: Kinematics.cpp:295
RobotDynamics::ReferenceFrame::getWorldFrame
static ReferenceFramePtr getWorldFrame()
Get a pointer to the world frame.
Definition: ReferenceFrame.hpp:263
RobotDynamics::ReferenceFramePtr
std::shared_ptr< ReferenceFrame > ReferenceFramePtr
Definition: ReferenceFrame.hpp:68
RobotDynamics::Math::VectorNd
Eigen::VectorXd VectorNd
Definition: rdl_eigenmath.hpp:20
RobotDynamics::Math::MatrixNd
Eigen::MatrixXd MatrixNd
Definition: rdl_eigenmath.hpp:21
RobotDynamics
Namespace for all structures of the RobotDynamics library.
Definition: examples.hpp:19
RobotDynamics::inverseKinematics
bool inverseKinematics(Model &model, const VectorNd &Qinit, const std::vector< unsigned int > &body_id, const std::vector< Vector3d > &body_point, const std::vector< Vector3d > &target_pos, VectorNd &Qres, double step_tol, double lambda, unsigned int max_iter)
Definition: Kinematics.cpp:1906
RobotDynamics::Model
Contains all information about the rigid body model.
Definition: Model.hpp:112
Generated by doxygen 1.9.1