Commit f21c6fa9 authored by Andrea Del Prete's avatar Andrea Del Prete
Browse files

Merge branch 'master' into pinocchio_v2

Conflicts:
	bindings/python/contacts/contact-6d.hpp
	exercizes/ex_1.py
	exercizes/ex_2.py
	unittest/tsid-formulation.cpp
parents 78fbb182 2e0c5491
*~
*.pyc
Xcode/
build*/
......@@ -123,6 +123,7 @@ SET(${PROJECT_NAME}_CONTACTS_HEADERS
include/tsid/contacts/fwd.hpp
include/tsid/contacts/contact-base.hpp
include/tsid/contacts/contact-6d.hpp
include/tsid/contacts/contact-point.hpp
)
SET(${PROJECT_NAME}_TRAJECTORIES_HEADERS
......@@ -162,6 +163,7 @@ SET(${PROJECT_NAME}_FORMULATIONS_HEADERS
SET(HEADERS
include/tsid/config.hpp
include/tsid/deprecation.hpp
include/tsid/utils/statistics.hpp
include/tsid/utils/stop-watch.hpp
include/tsid/utils/Stdafx.hh
......
import libtsid_pywrap as tsid
from libtsid_pywrap import *
from tsid.libtsid_pywrap import *
//
// Copyright (c) 2018 CNRS, NYU, MPI Tübingen
//
// This file is part of tsid
// tsid is free software: you can redistribute it
// and/or modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation, either version
// 3 of the License, or (at your option) any later version.
// tsid is distributed in the hope that it will be
// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
// General Lesser Public License for more details. You should have
// received a copy of the GNU Lesser General Public License along with
// tsid If not, see
// <http://www.gnu.org/licenses/>.
//
#include "tsid/bindings/python/contacts/contact-6d.hpp"
#include "tsid/bindings/python/contacts/expose-contact.hpp"
namespace tsid
{
namespace python
{
void exposeContact6d()
{
Contact6DPythonVisitor<tsid::contacts::Contact6d>::expose("Contact6d");
}
}
}
#ifndef __tsid_python_contact_hpp__
#define __tsid_python_contact_hpp__
#include <boost/python.hpp>
#include <string>
#include <eigenpy/eigenpy.hpp>
#include <boost/python/suite/indexing/vector_indexing_suite.hpp>
//
// Copyright (c) 2018 CNRS
// Copyright (c) 2018 CNRS, NYU, MPI Tübingen
//
// This file is part of tsid
// tsid is free software: you can redistribute it
......@@ -23,6 +15,14 @@
// <http://www.gnu.org/licenses/>.
//
#ifndef __tsid_python_contact_6d_hpp__
#define __tsid_python_contact_6d_hpp__
#include <boost/python.hpp>
#include <string>
#include <eigenpy/eigenpy.hpp>
#include <boost/python/suite/indexing/vector_indexing_suite.hpp>
#include "tsid/contacts/contact-6d.hpp"
#include "tsid/robots/robot-wrapper.hpp"
#include "tsid/math/constraint-inequality.hpp"
......@@ -37,8 +37,8 @@ namespace tsid
namespace bp = boost::python;
template<typename Contact6d>
struct ContactPythonVisitor
: public boost::python::def_visitor< ContactPythonVisitor<Contact6d> >
struct Contact6DPythonVisitor
: public boost::python::def_visitor< Contact6DPythonVisitor<Contact6d> >
{
template<class PyClass>
......@@ -46,35 +46,34 @@ namespace tsid
void visit(PyClass& cl) const
{
cl
.def(bp::init<std::string, robots::RobotWrapper &, std::string, Eigen::MatrixXd, Eigen::VectorXd, double, double, double, double> ((bp::arg("name"), bp::arg("robot"), bp::arg("framename"), bp::arg("contactPoint"), bp::arg("contactNormal"), bp::arg("frictionCoeff"), bp::arg("minForce"), bp::arg("maxForce"), bp::arg("regWeight")), "Default Constructor"))
.def(bp::init<std::string, robots::RobotWrapper &, std::string, Eigen::MatrixXd, Eigen::VectorXd, double, double, double> ((bp::arg("name"), bp::arg("robot"), bp::arg("framename"), bp::arg("contactPoint"), bp::arg("contactNormal"), bp::arg("frictionCoeff"), bp::arg("minForce"), bp::arg("maxForce")), "Default Constructor"))
.def(bp::init<std::string, robots::RobotWrapper &, std::string, Eigen::MatrixXd, Eigen::VectorXd, double, double, double, double> ((bp::arg("name"), bp::arg("robot"), bp::arg("framename"), bp::arg("contactPoint"), bp::arg("contactNormal"), bp::arg("frictionCoeff"), bp::arg("minForce"), bp::arg("maxForce"), bp::arg("wForceReg")), "Deprecated Constructor"))
.add_property("n_motion", &Contact6d::n_motion, "return number of motion")
.add_property("n_force", &Contact6d::n_force, "return number of force")
.add_property("name", &ContactPythonVisitor::name, "return name")
.def("computeMotionTask", &ContactPythonVisitor::computeMotionTask, bp::args("t", "q", "v", "data"))
.def("computeForceTask", &ContactPythonVisitor::computeForceTask, bp::args("t", "q", "v", "data"))
.def("computeForceRegularizationTask", &ContactPythonVisitor::computeForceRegularizationTask, bp::args("t", "q", "v", "data"))
.add_property("name", &Contact6DPythonVisitor::name, "return name")
.def("computeMotionTask", &Contact6DPythonVisitor::computeMotionTask, bp::args("t", "q", "v", "data"))
.def("computeForceTask", &Contact6DPythonVisitor::computeForceTask, bp::args("t", "q", "v", "data"))
.def("computeForceRegularizationTask", &Contact6DPythonVisitor::computeForceRegularizationTask, bp::args("t", "q", "v", "data"))
.add_property("getForceGeneratorMatrix", bp::make_function(&ContactPythonVisitor::getForceGeneratorMatrix, bp::return_value_policy<bp::copy_const_reference>()))
.add_property("getForceGeneratorMatrix", bp::make_function(&Contact6DPythonVisitor::getForceGeneratorMatrix, bp::return_value_policy<bp::copy_const_reference>()))
.add_property("getForceRegularizationWeight", &Contact6d::getForceRegularizationWeight, "return force reg weight")
.def("getNormalForce", &ContactPythonVisitor::getNormalForce, bp::arg("vec"))
.def("getNormalForce", &Contact6DPythonVisitor::getNormalForce, bp::arg("vec"))
.add_property("getMinNormalForce", &Contact6d::getMinNormalForce)
.add_property("getMaxNormalForce", &Contact6d::getMaxNormalForce)
.add_property("Kp", bp::make_function(&ContactPythonVisitor::Kp, bp::return_value_policy<bp::copy_const_reference>()))
.add_property("Kd", bp::make_function(&ContactPythonVisitor::Kd, bp::return_value_policy<bp::copy_const_reference>()))
.def("setKp", &ContactPythonVisitor::setKp, bp::arg("Kp"))
.def("setKd", &ContactPythonVisitor::setKd, bp::arg("Kd"))
.add_property("Kp", bp::make_function(&Contact6DPythonVisitor::Kp, bp::return_value_policy<bp::copy_const_reference>()))
.add_property("Kd", bp::make_function(&Contact6DPythonVisitor::Kd, bp::return_value_policy<bp::copy_const_reference>()))
.def("setKp", &Contact6DPythonVisitor::setKp, bp::arg("Kp"))
.def("setKd", &Contact6DPythonVisitor::setKd, bp::arg("Kd"))
.def("setContactPoints", &ContactPythonVisitor::setContactPoints, bp::args("vec"))
.def("setContactNormal", &ContactPythonVisitor::setContactNormal, bp::args("vec"))
.def("setFrictionCoefficient", &ContactPythonVisitor::setFrictionCoefficient, bp::args("friction_coeff"))
.def("setMinNormalForce", &ContactPythonVisitor::setMinNormalForce, bp::args("min_force"))
.def("setMaxNormalForce", &ContactPythonVisitor::setMaxNormalForce, bp::args("max_force"))
.def("setRegularizationTaskWeight", &ContactPythonVisitor::setRegularizationTaskWeight, bp::args("double"))
.def("setReference", &ContactPythonVisitor::setReference, bp::args("SE3"))
.def("setForceReference", &ContactPythonVisitor::setForceReference, bp::args("f_vec"))
.def("setRegularizationTaskWeightVector", &ContactPythonVisitor::setRegularizationTaskWeightVector, bp::args("w_vec"))
.def("setContactPoints", &Contact6DPythonVisitor::setContactPoints, bp::args("vec"))
.def("setContactNormal", &Contact6DPythonVisitor::setContactNormal, bp::args("vec"))
.def("setFrictionCoefficient", &Contact6DPythonVisitor::setFrictionCoefficient, bp::args("friction_coeff"))
.def("setMinNormalForce", &Contact6DPythonVisitor::setMinNormalForce, bp::args("min_force"))
.def("setMaxNormalForce", &Contact6DPythonVisitor::setMaxNormalForce, bp::args("max_force"))
.def("setReference", &Contact6DPythonVisitor::setReference, bp::args("SE3"))
.def("setForceReference", &Contact6DPythonVisitor::setForceReference, bp::args("f_vec"))
.def("setRegularizationTaskWeightVector", &Contact6DPythonVisitor::setRegularizationTaskWeightVector, bp::args("w_vec"))
;
}
static std::string name(Contact6d & self){
......@@ -128,9 +127,6 @@ namespace tsid
static bool setMaxNormalForce (Contact6d & self, const double maxNormalForce){
return self.setMaxNormalForce(maxNormalForce);
}
static bool setRegularizationTaskWeight (Contact6d & self, const double w){
return self.setRegularizationTaskWeight(w);
}
static void setReference(Contact6d & self, const pinocchio::SE3 & ref){
self.setReference(ref);
}
......@@ -150,11 +146,11 @@ namespace tsid
bp::class_<Contact6d>(class_name.c_str(),
doc.c_str(),
bp::no_init)
.def(ContactPythonVisitor<Contact6d>());
.def(Contact6DPythonVisitor<Contact6d>());
}
};
}
}
#endif // ifndef __tsid_python_contact_hpp__
\ No newline at end of file
#endif // ifndef __tsid_python_contact_hpp__
......@@ -15,16 +15,16 @@
// <http://www.gnu.org/licenses/>.
//
#include "tsid/bindings/python/contacts/contact.hpp"
#include "tsid/bindings/python/contacts/contact-point.hpp"
#include "tsid/bindings/python/contacts/expose-contact.hpp"
namespace tsid
{
namespace python
{
void exposeContact6d()
void exposeContactPoint()
{
ContactPythonVisitor<tsid::contacts::Contact6d>::expose("Contact6d");
ContactPointPythonVisitor<tsid::contacts::ContactPoint>::expose("ContactPoint");
}
}
}
//
// Copyright (c) 2018 CNRS
//
// This file is part of tsid
// tsid is free software: you can redistribute it
// and/or modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation, either version
// 3 of the License, or (at your option) any later version.
// tsid is distributed in the hope that it will be
// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
// General Lesser Public License for more details. You should have
// received a copy of the GNU Lesser General Public License along with
// tsid If not, see
// <http://www.gnu.org/licenses/>.
//
#ifndef __tsid_python_contact_6d_hpp__
#define __tsid_python_contact_6d_hpp__
#include <boost/python.hpp>
#include <string>
#include <eigenpy/eigenpy.hpp>
#include <boost/python/suite/indexing/vector_indexing_suite.hpp>
#include "tsid/contacts/contact-point.hpp"
#include "tsid/robots/robot-wrapper.hpp"
#include "tsid/math/constraint-inequality.hpp"
#include "tsid/math/constraint-equality.hpp"
#include "tsid/math/constraint-base.hpp"
namespace tsid
{
namespace python
{
namespace bp = boost::python;
template<typename ContactPoint>
struct ContactPointPythonVisitor
: public boost::python::def_visitor< ContactPointPythonVisitor<ContactPoint> >
{
template<class PyClass>
void visit(PyClass& cl) const
{
cl
.def(bp::init<std::string, robots::RobotWrapper &, std::string, Eigen::VectorXd, double, double, double> ((bp::arg("name"), bp::arg("robot"), bp::arg("framename"), bp::arg("contactNormal"), bp::arg("frictionCoeff"), bp::arg("minForce"), bp::arg("maxForce")), "Default Constructor"))
.add_property("n_motion", &ContactPoint::n_motion, "return number of motion")
.add_property("n_force", &ContactPoint::n_force, "return number of force")
.add_property("name", &ContactPointPythonVisitor::name, "return name")
.def("computeMotionTask", &ContactPointPythonVisitor::computeMotionTask, bp::args("t", "q", "v", "data"))
.def("computeForceTask", &ContactPointPythonVisitor::computeForceTask, bp::args("t", "q", "v", "data"))
.def("computeForceRegularizationTask", &ContactPointPythonVisitor::computeForceRegularizationTask, bp::args("t", "q", "v", "data"))
.add_property("getForceGeneratorMatrix", bp::make_function(&ContactPointPythonVisitor::getForceGeneratorMatrix, bp::return_value_policy<bp::copy_const_reference>()))
.def("getNormalForce", &ContactPointPythonVisitor::getNormalForce, bp::arg("vec"))
.add_property("getMinNormalForce", &ContactPoint::getMinNormalForce)
.add_property("getMaxNormalForce", &ContactPoint::getMaxNormalForce)
.add_property("Kp", bp::make_function(&ContactPointPythonVisitor::Kp, bp::return_value_policy<bp::copy_const_reference>()))
.add_property("Kd", bp::make_function(&ContactPointPythonVisitor::Kd, bp::return_value_policy<bp::copy_const_reference>()))
.def("setKp", &ContactPointPythonVisitor::setKp, bp::arg("Kp"))
.def("setKd", &ContactPointPythonVisitor::setKd, bp::arg("Kd"))
.def("useLocalFrame", &ContactPointPythonVisitor::useLocalFrame, bp::arg("local_frame"))
.def("setContactNormal", &ContactPointPythonVisitor::setContactNormal, bp::args("vec"))
.def("setFrictionCoefficient", &ContactPointPythonVisitor::setFrictionCoefficient, bp::args("friction_coeff"))
.def("setMinNormalForce", &ContactPointPythonVisitor::setMinNormalForce, bp::args("min_force"))
.def("setMaxNormalForce", &ContactPointPythonVisitor::setMaxNormalForce, bp::args("max_force"))
.def("setReference", &ContactPointPythonVisitor::setReference, bp::args("SE3"))
.def("setForceReference", &ContactPointPythonVisitor::setForceReference, bp::args("f_vec"))
.def("setRegularizationTaskWeightVector", &ContactPointPythonVisitor::setRegularizationTaskWeightVector, bp::args("w_vec"))
;
}
static std::string name(ContactPoint & self){
std::string name = self.name();
return name;
}
static math::ConstraintEquality computeMotionTask(ContactPoint & self, const double t, const Eigen::VectorXd & q, const Eigen::VectorXd & v, const pinocchio::Data & data){
self.computeMotionTask(t, q, v, data);
math::ConstraintEquality cons(self.getMotionConstraint().name(), self.getMotionConstraint().matrix(), self.getMotionConstraint().vector());
return cons;
}
static math::ConstraintInequality computeForceTask(ContactPoint & self, const double t, const Eigen::VectorXd & q, const Eigen::VectorXd & v, const pinocchio::Data & data){
self.computeForceTask(t, q, v, data);
math::ConstraintInequality cons(self.getForceConstraint().name(), self.getForceConstraint().matrix(), self.getForceConstraint().lowerBound(), self.getForceConstraint().upperBound());
return cons;
}
static math::ConstraintEquality computeForceRegularizationTask(ContactPoint & self, const double t, const Eigen::VectorXd & q, const Eigen::VectorXd & v, const pinocchio::Data & data){
self.computeForceRegularizationTask(t, q, v, data);
math::ConstraintEquality cons(self.getForceRegularizationTask().name(), self.getForceRegularizationTask().matrix(), self.getForceRegularizationTask().vector());
return cons;
}
static void useLocalFrame (ContactPoint & self, const bool local_frame) {
self.useLocalFrame(local_frame);
}
static const Eigen::MatrixXd & getForceGeneratorMatrix(ContactPoint & self){
return self.getForceGeneratorMatrix();
}
static const Eigen::VectorXd & Kp (ContactPoint & self){
return self.Kp();
}
static const Eigen::VectorXd & Kd (ContactPoint & self){
return self.Kd();
}
static void setKp (ContactPoint & self, const::Eigen::VectorXd Kp){
return self.Kp(Kp);
}
static void setKd (ContactPoint & self, const::Eigen::VectorXd Kd){
return self.Kd(Kd);
}
static bool setContactPoints (ContactPoint & self, const::Eigen::MatrixXd contactpoints){
return self.setContactPoints(contactpoints);
}
static bool setContactNormal (ContactPoint & self, const::Eigen::VectorXd contactNormal){
return self.setContactNormal(contactNormal);
}
static bool setFrictionCoefficient (ContactPoint & self, const double frictionCoefficient){
return self.setFrictionCoefficient(frictionCoefficient);
}
static bool setMinNormalForce (ContactPoint & self, const double minNormalForce){
return self.setMinNormalForce(minNormalForce);
}
static bool setMaxNormalForce (ContactPoint & self, const double maxNormalForce){
return self.setMaxNormalForce(maxNormalForce);
}
static void setReference(ContactPoint & self, const pinocchio::SE3 & ref){
self.setReference(ref);
}
static void setForceReference(ContactPoint & self, const::Eigen::VectorXd f_ref){
self.setForceReference(f_ref);
}
static void setRegularizationTaskWeightVector(ContactPoint & self, const::Eigen::VectorXd w){
self.setRegularizationTaskWeightVector(w);
}
static double getNormalForce(ContactPoint & self, Eigen::VectorXd f){
return self.getNormalForce(f);
}
static void expose(const std::string & class_name)
{
std::string doc = "ContactPoint info.";
bp::class_<ContactPoint>(class_name.c_str(),
doc.c_str(),
bp::no_init)
.def(ContactPointPythonVisitor<ContactPoint>());
}
};
}
}
#endif // ifndef __tsid_python_contact_hpp__
//
// Copyright (c) 2018 CNRS
// Copyright (c) 2018 CNRS, NYU, MPI Tübingen
//
// This file is part of tsid
// tsid is free software: you can redistribute it
......@@ -18,19 +18,22 @@
#ifndef __tsid_python_expose_contact_hpp__
#define __tsid_python_expose_contact_hpp__
#include "tsid/bindings/python/contacts/contact.hpp"
#include "tsid/bindings/python/contacts/contact-6d.hpp"
#include "tsid/bindings/python/contacts/contact-point.hpp"
namespace tsid
{
namespace python
{
void exposeContact6d();
void exposeContactPoint();
inline void exposeContact()
{
exposeContact6d();
exposeContactPoint();
}
} // namespace python
} // namespace tsid
#endif // ifndef __tsid_python_expose_contact_hpp__
//
// Copyright (c) 2018 CNRS
// Copyright (c) 2018 CNRS, NYU, MPI Tübingen
//
// This file is part of tsid
// tsid is free software: you can redistribute it
......@@ -26,6 +26,7 @@
#include "tsid/formulations/inverse-dynamics-formulation-acc-force.hpp"
#include "tsid/bindings/python/solvers/HQPData.hpp"
#include "tsid/contacts/contact-6d.hpp"
#include "tsid/contacts/contact-point.hpp"
#include "tsid/tasks/task-joint-posture.hpp"
#include "tsid/tasks/task-se3-equality.hpp"
#include "tsid/tasks/task-com-equality.hpp"
......@@ -56,7 +57,10 @@ namespace tsid
.def("updateTaskWeight", &InvDynPythonVisitor::updateTaskWeight, bp::args("task_name", "weight"))
.def("addRigidContact", &InvDynPythonVisitor::addRigidContact, bp::args("contact"))
.def("addRigidContact", &InvDynPythonVisitor::addRigidContact6dDeprecated, bp::args("contact"))
.def("addRigidContact", &InvDynPythonVisitor::addRigidContact6d, bp::args("contact", "force_reg_weight"))
.def("addRigidContact", &InvDynPythonVisitor::addRigidContactPoint, bp::args("contact", "force_reg_weight"))
.def("addRigidContact", &InvDynPythonVisitor::addRigidContactPointWithPriorityLevel, bp::args("contact", "force_reg_weight", "motion_weight", "priority_level"))
.def("removeTask", &InvDynPythonVisitor::removeTask, bp::args("task_name", "duration"))
.def("removeRigidContact", &InvDynPythonVisitor::removeRigidContact, bp::args("contact_name", "duration"))
.def("computeProblemData", &InvDynPythonVisitor::computeProblemData, bp::args("time", "q", "v"))
......@@ -84,9 +88,21 @@ namespace tsid
static bool updateTaskWeight(T& self, const std::string & task_name, double weight){
return self.updateTaskWeight(task_name, weight);
}
static bool addRigidContact(T& self, contacts::Contact6d & contact){
static bool addRigidContact6dDeprecated(T& self, contacts::Contact6d & contact){
return self.addRigidContact(contact);
}
}
static bool addRigidContact6d(T& self, contacts::Contact6d & contact, double force_regularization_weight){
return self.addRigidContact(contact, force_regularization_weight);
}
static bool addRigidContactPoint(T& self, contacts::ContactPoint & contact, double force_regularization_weight){
return self.addRigidContact(contact, force_regularization_weight);
}
static bool addRigidContactPointWithPriorityLevel(T& self, contacts::ContactPoint & contact,
double force_regularization_weight,
double motion_weight,
const bool priority_level){
return self.addRigidContact(contact, force_regularization_weight, motion_weight, priority_level);
}
static bool removeTask(T& self, const std::string & task_name, double transition_duration){
return self.removeTask(task_name, transition_duration);
}
......@@ -131,4 +147,4 @@ namespace tsid
}
#endif // ifndef __tsid_python_HQPOutput_hpp__
\ No newline at end of file
#endif // ifndef __tsid_python_HQPOutput_hpp__
//
// Copyright (c) 2018 CNRS
// Copyright (c) 2018 CNRS, NYU, MPI Tübingen
//
// This file is part of tsid
// tsid is free software: you can redistribute it
......@@ -40,7 +40,7 @@ namespace tsid
void visit(PyClass& cl) const
{
cl
.def(bp::init<>("Defulat Constructor"))
.def(bp::init<>("Default Constructor"))
.def("print_all", &T::print)
.def("append", &T::append_eq, bp::arg("data"))
.def("append", &T::append_ineq, bp::arg("data"))
......
......@@ -23,6 +23,7 @@
#include "tsid/bindings/python/tasks/task-joint-posture.hpp"
namespace tsid
{
namespace python
......
import pinocchio as se3
import tsid
import numpy as np
import numpy.matlib as matlib
from numpy import nan
from numpy.linalg import norm as norm
import os
import gepetto.corbaserver
import time
import commands
np.set_printoptions(precision=3, linewidth=200, suppress=True)
LINE_WIDTH = 60
print "".center(LINE_WIDTH,'#')
print " Test TSID with Quadruped Robot ".center(LINE_WIDTH, '#')
print "".center(LINE_WIDTH,'#'), '\n'
mu = 0.3 # friction coefficient
fMin = 1.0 # minimum normal force
fMax = 100.0 # maximum normal force
contact_frames = ['BL_contact', 'BR_contact', 'FL_contact', 'FR_contact']
contactNormal = np.matrix([0., 0., 1.]).T # direction of the normal to the contact surface
w_com = 1.0 # weight of center of mass task
w_posture = 1e-3 # weight of joint posture task
w_forceRef = 1e-5 # weight of force regularization task
kp_contact = 10.0 # proportional gain of contact constraint
kp_com = 10.0 # proportional gain of center of mass task
kp_posture = 10.0 # proportional gain of joint posture task
dt = 0.001 # controller time step
PRINT_N = 500 # print every PRINT_N time steps
DISPLAY_N = 25 # update robot configuration in viwewer every DISPLAY_N time steps
N_SIMULATION = 6000 # number of time steps simulated
filename = str(os.path.dirname(os.path.abspath(__file__)))
path = filename + '/../models'
urdf = path + '/quadruped/urdf/quadruped.urdf'
vector = se3.StdVec_StdString()
vector.extend(item for item in path)
robot = tsid.RobotWrapper(urdf, vector, se3.JointModelFreeFlyer(), False)
# for gepetto viewer
robot_display = se3.RobotWrapper.BuildFromURDF(urdf, [path, ], se3.JointModelFreeFlyer())
l = commands.getstatusoutput("ps aux |grep 'gepetto-gui'|grep -v 'grep'|wc -l")
if int(l[1]) == 0:
os.system('gepetto-gui &')
time.sleep(1)
cl = gepetto.corbaserver.Client()
gui = cl.gui
robot_display.initDisplay(loadModel=True)
#q = se3.getNeutralConfigurationFromSrdf(robot.model(), srdf, False)
q = robot_display.model.neutralConfiguration #matlib.zeros(robot.nq).T
q[2] += 0.5
for i in range(4):
q[7 + 2*i] = -0.8
q[8 + 2*i] = 1.6
v = matlib.zeros(robot.nv).T
robot_display.displayCollisions(False)
robot_display.displayVisuals(True)
robot_display.display(q)
assert [robot.model().existFrame(name) for name in contact_frames]
t = 0.0 # time
invdyn = tsid.InverseDynamicsFormulationAccForce("tsid", robot, False)
invdyn.computeProblemData(t, q, v)
data = invdyn.data()
# Place the robot onto the ground.
id_contact = robot_display.model.getFrameId(contact_frames[0])
q[2] -= robot.framePosition(data, id_contact).translation[2, 0]
robot.computeAllTerms(data, q, v)
contacts = 4*[None]
for i, name in enumerate(contact_frames):
contacts[i] =tsid.ContactPoint(name, robot, name, contactNormal, mu, fMin, fMax)
contacts[i].setKp(kp_contact * matlib.ones(3).T)
contacts[i].setKd(2.0 * np.sqrt(kp_contact) * matlib.ones(3).T)
H_rf_ref = robot.framePosition(data, robot.model().getFrameId(name))
contacts[i].setReference(H_rf_ref)
contacts[i].useLocalFrame(False)
invdyn.addRigidContact(contacts[i], w_forceRef, 1.0, 1)
comTask = tsid.TaskComEquality("task-com", robot)
comTask.setKp(kp_com * matlib.ones(3).T)
comTask.setKd(2.0 * np.sqrt(kp_com) * matlib.ones(3).T)
invdyn.addMotionTask(comTask, w_com, 1, 0.0)
postureTask = tsid.TaskJointPosture("task-posture", robot)
postureTask.setKp(kp_posture * matlib.ones(robot.nv-6).T)
postureTask.setKd(2.0 * np.sqrt(kp_posture) * matlib.ones(robot.nv-6).T)
invdyn.addMotionTask(postureTask, w_posture, 1, 0.0)
com_ref = robot.com(data)
trajCom = tsid.TrajectoryEuclidianConstant("traj_com", com_ref)
sampleCom = trajCom.computeNext()
q_ref = q[7:]
trajPosture = tsid.TrajectoryEuclidianConstant("traj_joint", q_ref)
solver = tsid.SolverHQuadProgFast("qp solver")
solver.resize(invdyn.nVar, invdyn.nEq, invdyn.nIn)
com_pos = matlib.empty((3, N_SIMULATION))*nan
com_vel = matlib.empty((3, N_SIMULATION))*nan
com_acc = matlib.empty((3, N_SIMULATION))*nan