Commit ac2cba4a by Justin Carpentier Committed by GitHub

### Merge pull request #501 from proyan/devel

Documentation and unittest for forward dynamics with damped cholesky inverse
parents cc8b8d7b c5942ca9
 ... @@ -37,6 +37,7 @@ namespace se3 ... @@ -37,6 +37,7 @@ namespace se3 /// \text{s.t.} & & J (q) \ddot{q} + \gamma (q, \dot{q}) = 0 \end{eqnarray} \f$/// \text{s.t.} & & J (q) \ddot{q} + \gamma (q, \dot{q}) = 0 \end{eqnarray} \f$
/// where \f$\ddot{q}_{\text{free}} \f$ is the free acceleration (i.e. without constraints), /// where \f$\ddot{q}_{\text{free}} \f$ is the free acceleration (i.e. without constraints), /// \f$M \f$ is the mass matrix, \f$J \f$ the constraint Jacobian and \f$\gamma \f$ is the constraint drift. /// \f$M \f$ is the mass matrix, \f$J \f$ the constraint Jacobian and \f$\gamma \f$ is the constraint drift. /// By default, the constraint Jacobian is assumed to be full rank, and undamped Cholesky inverse is performed. /// /// /// \param[in] model The model structure of the rigid body system. /// \param[in] model The model structure of the rigid body system. /// \param[in] data The data structure of the rigid body system. /// \param[in] data The data structure of the rigid body system. ... @@ -45,7 +46,9 @@ namespace se3 ... @@ -45,7 +46,9 @@ namespace se3 /// \param[in] tau The joint torque vector (dim model.nv). /// \param[in] tau The joint torque vector (dim model.nv). /// \param[in] J The Jacobian of the constraints (dim nb_constraints*model.nv). /// \param[in] J The Jacobian of the constraints (dim nb_constraints*model.nv). /// \param[in] gamma The drift of the constraints (dim nb_constraints). /// \param[in] gamma The drift of the constraints (dim nb_constraints). /// \param[in] updateKinematics If true, the algorithm calls first se3::computeAllTerms. Otherwise, it uses the current dynamic values stored in data. /// \param[in] inv_damping Damping factor for cholesky decomposition of JMinvJt. Set to zero if constraints are full rank. /// \param[in] updateKinematics If true, the algorithm calls first se3::computeAllTerms. Otherwise, it uses the current dynamic values stored in data. \\ /// \note A hint: 1e-12 as the damping factor gave good result in the particular case of redundancy in contact constraints on the two feet. /// /// /// \return A reference to the joint acceleration stored in data.ddq. The Lagrange Multipliers linked to the contact forces are available throw data.lambda_c vector. /// \return A reference to the joint acceleration stored in data.ddq. The Lagrange Multipliers linked to the contact forces are available throw data.lambda_c vector. /// /// ... ...
 ... @@ -91,6 +91,62 @@ BOOST_AUTO_TEST_CASE ( test_FD ) ... @@ -91,6 +91,62 @@ BOOST_AUTO_TEST_CASE ( test_FD ) Eigen::VectorXd dynamics_residual (data.M * data.ddq + data.nle - tau - J.transpose()*data.lambda_c); Eigen::VectorXd dynamics_residual (data.M * data.ddq + data.nle - tau - J.transpose()*data.lambda_c); BOOST_CHECK(dynamics_residual.norm() <= 1e-12); BOOST_CHECK(dynamics_residual.norm() <= 1e-12); } BOOST_AUTO_TEST_CASE ( test_FD_with_damping ) { using namespace Eigen; using namespace se3; se3::Model model; se3::buildModels::humanoidSimple(model,true); se3::Data data(model); VectorXd q = VectorXd::Ones(model.nq); q.segment <4> (3).normalize(); se3::computeJointJacobians(model, data, q); VectorXd v = VectorXd::Ones(model.nv); VectorXd tau = VectorXd::Zero(model.nv); const std::string RF = "rleg6_joint"; Data::Matrix6x J_RF (6, model.nv); J_RF.setZero(); getJointJacobian (model, data, model.getJointId(RF), J_RF); Eigen::MatrixXd J (12, model.nv); J.setZero(); J.topRows<6> () = J_RF; J.bottomRows<6> () = J_RF; Eigen::VectorXd gamma (VectorXd::Ones(12)); // Forward Dynamics with damping se3::forwardDynamics(model, data, q, v, tau, J, gamma, 1e-12,true); // Matrix Definitions Eigen::MatrixXd H(J.transpose()); data.M.triangularView() = data.M.transpose().triangularView(); MatrixXd Minv (data.M.inverse()); MatrixXd JMinvJt (J * Minv * J.transpose()); // Check that JMinvJt is correctly formed Eigen::MatrixXd G_ref(J.transpose()); cholesky::Uiv(model, data, G_ref); for(int k=0;k
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!