Unverified Commit dab03d38 authored by Guilhem Saurel's avatar Guilhem Saurel Committed by GitHub
Browse files

Merge branch 'devel' into pinocchio_v2

parents a67e8cf5 8223cbe1
......@@ -132,4 +132,4 @@ namespace tsid
}
#endif // ifndef __tsid_python_task_com_hpp__
\ No newline at end of file
#endif // ifndef __tsid_python_task_com_hpp__
......@@ -140,4 +140,4 @@ namespace tsid
}
#endif // ifndef __tsid_python_task_joint_hpp__
\ No newline at end of file
#endif // ifndef __tsid_python_task_joint_hpp__
......@@ -136,4 +136,4 @@ namespace tsid
}
#endif // ifndef __tsid_python_task_se3_hpp__
\ No newline at end of file
#endif // ifndef __tsid_python_task_se3_hpp__
......@@ -81,4 +81,4 @@ namespace tsid
}
#endif // ifndef __tsid_python_traj_se3_hpp__
\ No newline at end of file
#endif // ifndef __tsid_python_traj_se3_hpp__
Subproject commit 8e7bedfcbd8524c0401a58fd74edc07c3d4308d0
Subproject commit 1d9aeca25e970d2d967fd5be0fb93fe961db121b
......@@ -28,6 +28,7 @@
#include <string>
#include <vector>
namespace tsid
{
namespace robots
......
......@@ -60,7 +60,7 @@ Contact6d::Contact6d(const std::string & name,
const double frictionCoefficient,
const double minNormalForce,
const double maxNormalForce,
const double forceRegWeight):
const double ):
Contact6d(name, robot, frameName, contactPoints, contactNormal, frictionCoefficient, minNormalForce, maxNormalForce)
{
std::cout<<"[Contact6d] The constructor with forceRegWeight is deprecated now. forceRegWeight should now be specified when calling addRigidContact()\n";
......
......@@ -71,8 +71,9 @@ namespace tsid
data.M.triangularView<Eigen::StrictlyLower>()
= data.M.transpose().triangularView<Eigen::StrictlyLower>();
// computeAllTerms does not compute the com acceleration, so we need to call centerOfMass
pinocchio::centerOfMass(m_model, data, 2, false);
pinocchio::updateFramePlacements(m_model, data);
// Check this line, calling with zero acceleration at the last phase compute the CoM acceleration.
// pinocchio::centerOfMass(m_model, data, q,v,false);
pinocchio::framesForwardKinematics(m_model, data);
pinocchio::centerOfMass(m_model, data, q, v, Eigen::VectorXd::Zero(nv()));
}
......
......@@ -77,7 +77,7 @@ BOOST_AUTO_TEST_CASE ( test_contact_6d )
BOOST_CHECK(contact.Kp().isApprox(Kp));
BOOST_CHECK(contact.Kd().isApprox(Kd));
Vector q = robot.model().neutralConfiguration;
Vector q = neutral(robot.model());
Vector v = Vector::Zero(robot.nv());
pinocchio::Data data(robot.model());
robot.computeAllTerms(data, q, v);
......
......@@ -83,7 +83,7 @@ BOOST_AUTO_TEST_CASE ( test_task_se3_equality )
const double dt = 0.001;
MatrixXd Jpinv(robot.nv(), 6);
double error, error_past=1e100;
VectorXd q = robot.model().neutralConfiguration;
VectorXd q = neutral(robot.model());
VectorXd v = VectorXd::Zero(robot.nv());
pinocchio::Data data(robot.model());
for(int i=0; i<max_it; i++)
......@@ -138,10 +138,11 @@ BOOST_AUTO_TEST_CASE ( test_task_com_equality )
pinocchio::Data data(robot.model());
const string srdfFileName = package_dirs[0] + "/srdf/romeo_collision.srdf";
pinocchio::srdf::getNeutralConfigurationFromSrdf(robot.model(),srdfFileName);
pinocchio::srdf::loadReferenceConfigurations(robot.model(),srdfFileName,false);
// const unsigned int nv = robot.nv();
VectorXd q = robot.model().neutralConfiguration;
VectorXd q = neutral(robot.model());
std::cout << "q: " << q.transpose() << std::endl;
q(2) += 0.84;
......@@ -234,7 +235,7 @@ BOOST_AUTO_TEST_CASE ( test_task_joint_posture )
const double dt = 0.001;
MatrixXd Jpinv(robot.nv(), na);
double error, error_past=1e100;
VectorXd q = robot.model().neutralConfiguration;
VectorXd q = neutral(robot.model());
VectorXd v = VectorXd::Zero(robot.nv());
pinocchio::Data data(robot.model());
for(int i=0; i<max_it; i++)
......
......@@ -112,10 +112,11 @@ class StandardRomeoInvDynCtrl
robot = new RobotWrapper(urdfFileName, package_dirs, pinocchio::JointModelFreeFlyer());
const string srdfFileName = package_dirs[0] + "/srdf/romeo_collision.srdf";
pinocchio::srdf::getNeutralConfigurationFromSrdf(robot->model(),srdfFileName);
pinocchio::srdf::loadReferenceConfigurations(robot->model(),srdfFileName,false);
const unsigned int nv = robot->nv();
q = robot->model().neutralConfiguration;
q = neutral(robot->model());
std::cout << "q: " << q.transpose() << std::endl;
q(2) += 0.84;
v = Vector::Zero(nv);
......@@ -504,7 +505,7 @@ BOOST_AUTO_TEST_CASE ( test_contact_point_invdyn_formulation_acc_force )
BOOST_REQUIRE(robot.model().existFrame(frameName));
Vector q = robot.model().neutralConfiguration;
Vector q = neutral(robot.model());
Vector v = Vector::Zero(robot.nv());
const unsigned int nv = robot.nv();
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment