Kinematics.h
Go to the documentation of this file.
1 /*
2  * Original Copyright (c) 2011-2016 Martin Felis <martin.felis@iwr.uni-heidelberg.de>
3  *
4  *
5  * RDL - Robot Dynamics Library
6  * Modifications Copyright (c) 2017 Jordan Lack <jlack1987@gmail.com>
7  *
8  * Licensed under the zlib license. See LICENSE for more details.
9  */
10 
11 #ifndef RDL_KINEMATICS_H
12 #define RDL_KINEMATICS_H
13 
18 #include <assert.h>
19 #include <iostream>
20 #include "rdl_dynamics/FrameVectorPair.hpp"
21 #include "rdl_dynamics/Model.h"
22 
23 namespace RobotDynamics
24 {
53 void updateKinematics(Model& model, const Math::VectorNd& Q, const Math::VectorNd& QDot, const Math::VectorNd& QDDot);
54 
72 void updateKinematicsCustom(Model& model, const Math::VectorNd* Q, const Math::VectorNd* QDot = nullptr, const Math::VectorNd* QDDot = nullptr);
73 
95 void updateKinematicsParallel(Model& model, const Math::VectorNd& Q, const Math::VectorNd& QDot, const Math::VectorNd& QDDot);
96 
125 void updateKinematicsCustomParallel(Model& model, const Math::VectorNd* Q, const Math::VectorNd* QDot = nullptr, const Math::VectorNd* QDDot = nullptr);
126 
134 void updateAccelerations(Model& model, const Math::VectorNd& QDDot);
135 
156 void calcPointJacobian(Model& model, const Math::VectorNd& Q, unsigned int body_id, const Math::Vector3d& point_position, Math::MatrixNd& G,
157  bool update_kinematics = true);
158 
174 void calcPointJacobian(Model& model, const Math::VectorNd& Q, Math::MatrixNd& G, ReferenceFramePtr frame, bool update_kinematics = true);
175 
192 void calcPointJacobian6D(Model& model, const Math::VectorNd& Q, Math::MatrixNd& G, ReferenceFramePtr frame, bool update_kinematics = true);
193 
209 void calcRelativePointJacobian6D(Model& model, const Math::VectorNd& Q, Math::MatrixNd& G, ReferenceFramePtr baseFrame, ReferenceFramePtr relativeFrame,
210  ReferenceFramePtr expressedInFrame = RobotDynamics::ReferenceFrame::getWorldFrame(), bool update_kinematics = true);
227 void calcRelativePointJacobianDot6D(Model& model, const Math::VectorNd& Q, const Math::VectorNd& QDot, Math::MatrixNd& G, ReferenceFramePtr baseFrame,
228  ReferenceFramePtr relativeFrame, ReferenceFramePtr expressedInFrame = RobotDynamics::ReferenceFrame::getWorldFrame(),
229  bool update_kinematics = true);
230 
254 void calcRelativePointJacobianAndJacobianDot6D(Model& model, const Math::VectorNd& Q, const Math::VectorNd& QDot, Math::MatrixNd& G, Math::MatrixNd& GDot,
255  ReferenceFramePtr baseFrame, ReferenceFramePtr relativeFrame,
256  ReferenceFramePtr expressedInFrame = RobotDynamics::ReferenceFrame::getWorldFrame(), bool update_kinematics = true);
257 
279 void calcRelativeBodySpatialJacobian(Model& model, const Math::VectorNd& Q, Math::MatrixNd& G, ReferenceFramePtr baseFrame, ReferenceFramePtr relativeFrame,
280  ReferenceFramePtr expressedInFrame = nullptr, bool update_kinematics = true);
281 
300 void calcRelativeBodySpatialJacobianDot(Model& model, const Math::VectorNd& Q, const Math::VectorNd& QDot, Math::MatrixNd& G, ReferenceFramePtr baseFrame,
301  ReferenceFramePtr relativeFrame, ReferenceFramePtr expressedInFrame = nullptr, bool update_kinematics = true);
302 
318 void calcRelativeBodySpatialJacobianAndJacobianDot(Model& model, const Math::VectorNd& Q, const Math::VectorNd& QDot, Math::MatrixNd& G, Math::MatrixNd& GDot,
319  ReferenceFramePtr baseFrame, ReferenceFramePtr relativeFrame, ReferenceFramePtr expressedInFrame = nullptr,
320  bool update_kinematics = true);
321 
343 void calcPointJacobianDot(Model& model, const Math::VectorNd& Q, const Math::VectorNd& QDot, unsigned int body_id, const Math::Vector3d& point_position,
344  Math::MatrixNd& G, bool update_kinematics = true);
345 
366 void calcPointJacobianDot6D(Model& model, const Math::VectorNd& Q, const Math::VectorNd& QDot, RobotDynamics::ReferenceFramePtr frame, Math::MatrixNd& G,
367  bool update_kinematics = true);
368 
389 void calcPointJacobianDot(Model& model, const Math::VectorNd& Q, const Math::VectorNd& QDot, RobotDynamics::ReferenceFramePtr frame, Math::MatrixNd& G,
390  bool update_kinematics = true);
391 
412 void calcPointJacobian6D(Model& model, const Math::VectorNd& Q, unsigned int body_id, const Math::Vector3d& point_position, Math::MatrixNd& G,
413  bool update_kinematics = true);
414 
439 void calcPointJacobianDot6D(Model& model, const Math::VectorNd& Q, const Math::VectorNd& QDot, unsigned int body_id, const Math::Vector3d& point_position,
440  Math::MatrixNd& G, bool update_kinematics = true);
441 
471 void calcBodySpatialJacobian(Model& model, const Math::VectorNd& Q, unsigned int body_id, Math::MatrixNd& G, bool update_kinematics = true);
472 
492 void calcBodySpatialJacobianDot(Model& model, const Math::VectorNd& Q, const Math::VectorNd QDot, unsigned int body_id, Math::MatrixNd& G,
493  const bool update_kinematics = true);
494 
506 Math::FrameVector calcPointVelocity(Model& model, const Math::VectorNd& Q, const Math::VectorNd& QDot, unsigned int body_id, const Math::Vector3d& point_position,
507  bool update_kinematics = true);
508 
520 Math::FrameVectorPair calcPointVelocity6D(Model& model, const Math::VectorNd& Q, const Math::VectorNd& QDot, unsigned int body_id, const Math::Vector3d& point_position,
521  bool update_kinematics = true);
522 
534 Math::FrameVectorPair calcPointVelocity6D(Model& model, const Math::VectorNd& Q, const Math::VectorNd& QDot, RobotDynamics::ReferenceFramePtr frame,
535  bool update_kinematics = true);
536 
552 Math::FrameVectorPair calcPointVelocity6D(Model& model, const Math::VectorNd& Q, const Math::VectorNd& QDot, RobotDynamics::ReferenceFramePtr baseFrame,
553  RobotDynamics::ReferenceFramePtr relativeFrame,
554  RobotDynamics::ReferenceFramePtr expressedInFrame = RobotDynamics::ReferenceFrame::getWorldFrame(),
555  bool update_kinematics = true);
556 
579 Math::FrameVector calcPointAcceleration(Model& model, const Math::VectorNd& Q, const Math::VectorNd& QDot, const Math::VectorNd& QDDot, unsigned int body_id,
580  const Math::Vector3d& point_position, bool update_kinematics = true);
581 
605 Math::SpatialMotion calcSpatialVelocity(Model& model, const Math::VectorNd& Q, const Math::VectorNd& QDot, ReferenceFramePtr body_frame,
606  ReferenceFramePtr relative_body_frame, ReferenceFramePtr expressedInFrame = nullptr, const bool update_kinematics = true);
607 
631 Math::SpatialMotion calcSpatialVelocity(Model& model, const Math::VectorNd& Q, const Math::VectorNd& QDot, const unsigned int body_id,
632  const unsigned int relative_body_id, ReferenceFramePtr expressedInFrame = nullptr, const bool update_kinematics = true);
633 
655 Math::SpatialAcceleration calcSpatialAcceleration(Model& model, const Math::VectorNd& Q, const Math::VectorNd& QDot, const Math::VectorNd& QDDot,
656  const unsigned int body_id, const unsigned int relative_body_id, ReferenceFramePtr expressedInFrame = nullptr,
657  const bool update_kinematics = true);
658 
680 Math::SpatialAcceleration calcSpatialAcceleration(Model& model, const Math::VectorNd& Q, const Math::VectorNd& QDot, const Math::VectorNd& QDDot,
681  ReferenceFramePtr body_frame, ReferenceFramePtr relative_body_frame, ReferenceFramePtr expressedInFrame = nullptr,
682  const bool update_kinematics = true);
683 
705 Math::FrameVectorPair calcPointAcceleration6D(Model& model, const Math::VectorNd& Q, const Math::VectorNd& QDot, const Math::VectorNd& QDDot, unsigned int body_id,
706  const Math::Vector3d& point_position, bool update_kinematics = true);
707 
732 Math::FrameVectorPair calcPointAcceleration6D(Model& model, const Math::VectorNd& Q, const Math::VectorNd& QDot, const Math::VectorNd& QDDot,
733  ReferenceFramePtr body_frame, ReferenceFramePtr relative_body_frame, ReferenceFramePtr expressedInFrame = nullptr,
734  const bool update_kinematics = true);
735 
768 bool inverseKinematics(Model& model, const Math::VectorNd& Qinit, const std::vector<unsigned int>& body_id, const std::vector<Math::Vector3d>& body_point,
769  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);
770 
772 } // namespace RobotDynamics
773 
774 /* RDL_KINEMATICS_H */
775 #endif // ifndef RDL_KINEMATICS_H
RobotDynamics::calcPointJacobian
void calcPointJacobian(Model &model, const Math::VectorNd &Q, unsigned int body_id, const Math::Vector3d &point_position, Math::MatrixNd &G, bool update_kinematics=true)
Computes the 3D point jacobian for a point on a body.
Definition: Kinematics.cc:510
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=nullptr, bool update_kinematics=true)
Computes both the body spatial jacobian and its time derivative. This function will be a bit more eff...
Definition: Kinematics.cc:402
QDDot
VectorNd QDDot
Definition: RdlTwoLegModelTests.cpp:31
RobotDynamics::calcSpatialVelocity
Math::SpatialMotion calcSpatialVelocity(Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDot, ReferenceFramePtr body_frame, ReferenceFramePtr relative_body_frame, ReferenceFramePtr expressedInFrame=nullptr, const bool update_kinematics=true)
Compute the spatial velocity of any frame with respect to any other frame, expressed in an arbirtary ...
Definition: Kinematics.cc:1670
QDot
VectorNd QDot
Definition: RdlTwoLegModelTests.cpp:30
RobotDynamics::calcRelativeBodySpatialJacobian
void calcRelativeBodySpatialJacobian(Model &model, const Math::VectorNd &Q, Math::MatrixNd &G, ReferenceFramePtr baseFrame, ReferenceFramePtr relativeFrame, ReferenceFramePtr expressedInFrame=nullptr, bool update_kinematics=true)
Computes the jacobian of a frame with being the "base" frame, being the "relative" frame...
Definition: Kinematics.cc:234
Q
VectorNd Q
Definition: RdlTwoLegModelTests.cpp:29
RobotDynamics::calcPointJacobianDot
void calcPointJacobianDot(Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDot, unsigned int body_id, const Math::Vector3d &point_position, Math::MatrixNd &G, bool update_kinematics=true)
Computes the time derivative of the linear components the a point jacobian on a body.
Definition: Kinematics.cc:751
RobotDynamics::calcRelativePointJacobian6D
void calcRelativePointJacobian6D(Model &model, const Math::VectorNd &Q, Math::MatrixNd &G, ReferenceFramePtr baseFrame, ReferenceFramePtr relativeFrame, ReferenceFramePtr expressedInFrame=RobotDynamics::ReferenceFrame::getWorldFrame(), bool update_kinematics=true)
Compute the 6D jacobian of the origin of a reference frame relative to the origin of another referenc...
Definition: Kinematics.cc:643
RobotDynamics::calcPointAcceleration
Math::FrameVector 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)
Computes the linear acceleration of a point on a body.
Definition: Kinematics.cc:1593
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=RobotDynamics::ReferenceFrame::getWorldFrame(), bool update_kinematics=true)
Compute the point Jacobian of the origin of baseFrame, relative to the origin of relativeFrame, expressed in expressedInFrame. Also compute that point Jacobians time derivative.
Definition: Kinematics.cc:1033
RobotDynamics::ReferenceFrame::getWorldFrame
static ReferenceFramePtr getWorldFrame()
Get a pointer to the world frame.
Definition: ReferenceFrame.hpp:267
RobotDynamics::calcSpatialAcceleration
Math::SpatialAcceleration calcSpatialAcceleration(Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDot, const Math::VectorNd &QDDot, const unsigned int body_id, const unsigned int relative_body_id, ReferenceFramePtr expressedInFrame=nullptr, const bool update_kinematics=true)
Compute the spatial acceleration of any body with respect to any other body and express the result in...
Definition: Kinematics.cc:1852
RobotDynamics::calcPointJacobian6D
void calcPointJacobian6D(Model &model, const Math::VectorNd &Q, Math::MatrixNd &G, ReferenceFramePtr frame, bool update_kinematics=true)
Compute the 6D point jacobian of the origin of a reference frame If a position of a point is computed...
Definition: Kinematics.cc:604
RobotDynamics::ReferenceFramePtr
std::shared_ptr< ReferenceFrame > ReferenceFramePtr
Definition: ReferenceFrame.hpp:72
RobotDynamics::calcPointVelocity
Math::FrameVector calcPointVelocity(Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDot, unsigned int body_id, const Math::Vector3d &point_position, bool update_kinematics=true)
Computes the velocity of a point on a body.
Definition: Kinematics.cc:1422
RobotDynamics::calcRelativePointJacobianDot6D
void calcRelativePointJacobianDot6D(Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDot, Math::MatrixNd &G, ReferenceFramePtr baseFrame, ReferenceFramePtr relativeFrame, ReferenceFramePtr expressedInFrame=RobotDynamics::ReferenceFrame::getWorldFrame(), bool update_kinematics=true)
Compute the time derivative of the 6D jacobian of the origin of a reference frame relative to the ori...
Definition: Kinematics.cc:877
RobotDynamics::calcPointJacobianDot6D
void calcPointJacobianDot6D(Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDot, RobotDynamics::ReferenceFramePtr frame, Math::MatrixNd &G, bool update_kinematics=true)
Computes the time derivative of a point jacobian of a point on a body.
Definition: Kinematics.cc:826
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=true)
Computes the time derivative of the spatial jacobian for a body. The result will be returned via the ...
Definition: Kinematics.cc:1367
RobotDynamics::updateKinematicsParallel
void updateKinematicsParallel(Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDot, const Math::VectorNd &QDDot)
Updates and computes velocities and accelerations of the bodies.
Definition: Kinematics.cc:214
RobotDynamics::Math::VectorNd
Eigen::VectorXd VectorNd
Definition: rdl_eigenmath.h:25
RobotDynamics::calcBodySpatialJacobian
void calcBodySpatialJacobian(Model &model, const Math::VectorNd &Q, unsigned int body_id, Math::MatrixNd &G, bool update_kinematics=true)
Computes the spatial jacobian for a body. The result will be returned via the G argument and represen...
Definition: Kinematics.cc:1310
RobotDynamics::updateKinematics
void updateKinematics(Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDot, const Math::VectorNd &QDDot)
Updates and computes velocities and accelerations of the bodies.
Definition: Kinematics.cc:25
RobotDynamics::inverseKinematics
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=50)
Computes the inverse kinematics iteratively using a damped Levenberg-Marquardt method (also known as ...
RobotDynamics::calcRelativeBodySpatialJacobianDot
void calcRelativeBodySpatialJacobianDot(Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDot, Math::MatrixNd &G, ReferenceFramePtr baseFrame, ReferenceFramePtr relativeFrame, ReferenceFramePtr expressedInFrame=nullptr, bool update_kinematics=true)
Computes the rate of change of the jacobian, , with being the "base" frame, being the relative" fra...
Definition: Kinematics.cc:301
RobotDynamics::updateKinematicsCustom
void updateKinematicsCustom(Model &model, const Math::VectorNd *Q, const Math::VectorNd *QDot=nullptr, const Math::VectorNd *QDDot=nullptr)
Selectively updates model internal states of body positions, velocities and/or accelerations.
Definition: Kinematics.cc:72
RobotDynamics::Math::MatrixNd
Eigen::MatrixXd MatrixNd
Definition: rdl_eigenmath.h:26
RobotDynamics::calcPointAcceleration6D
Math::FrameVectorPair 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)
Computes linear and angular acceleration of a point on a body.
Definition: Kinematics.cc:1631
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=true)
Computes angular and linear velocity of a point that is fixed on a body.
Definition: Kinematics.cc:1459
RobotDynamics::updateAccelerations
void updateAccelerations(Model &model, const Math::VectorNd &QDDot)
Computes only the accelerations of the bodies.
Definition: Kinematics.cc:152
RobotDynamics::updateKinematicsCustomParallel
void updateKinematicsCustomParallel(Model &model, const Math::VectorNd *Q, const Math::VectorNd *QDot=nullptr, const Math::VectorNd *QDDot=nullptr)
Selectively updates model internal states of body positions, velocities and/or accelerations and spaw...
Definition: Kinematics.cc:194
RobotDynamics
Namespace for all structures of the RobotDynamics library.
Definition: Body.h:21

rdl_dynamics
Author(s):
autogenerated on Tue Apr 20 2021 02:25:27