Commit 2e0c5491 authored by Andrea Del Prete's avatar Andrea Del Prete
Browse files

Introduce weight for motion task associated to RigidContact. Specify weight...

Introduce weight for motion task associated to RigidContact. Specify weight (for both motion task and force regularization task) when calling addRigidContact rather than when creating Contact object (to be coherent with weights of other Task's).
parent 2c2e3f3e
*~
*.pyc
Xcode/
build*/
Subproject commit 083fa2cb0fa4ad594926d9bb3d246075e62ce9ee
Subproject commit dc8b946d456d2c41ad12b819111b005148c68031
......@@ -46,7 +46,8 @@ 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", &Contact6DPythonVisitor::name, "return name")
......@@ -56,7 +57,6 @@ namespace tsid
.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", &Contact6DPythonVisitor::getNormalForce, bp::arg("vec"))
.add_property("getMinNormalForce", &Contact6d::getMinNormalForce)
.add_property("getMaxNormalForce", &Contact6d::getMaxNormalForce)
......@@ -71,7 +71,6 @@ namespace tsid
.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("setRegularizationTaskWeight", &Contact6DPythonVisitor::setRegularizationTaskWeight, bp::args("double"))
.def("setReference", &Contact6DPythonVisitor::setReference, bp::args("SE3"))
.def("setForceReference", &Contact6DPythonVisitor::setForceReference, bp::args("f_vec"))
.def("setRegularizationTaskWeightVector", &Contact6DPythonVisitor::setRegularizationTaskWeightVector, bp::args("w_vec"))
......@@ -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 se3::SE3 & ref){
self.setReference(ref);
}
......@@ -157,4 +153,4 @@ namespace tsid
}
#endif // ifndef __tsid_python_contact_hpp__
\ No newline at end of file
#endif // ifndef __tsid_python_contact_hpp__
......@@ -45,7 +45,7 @@ namespace tsid
void visit(PyClass& cl) const
{
cl
.def(bp::init<std::string, robots::RobotWrapper &, std::string, Eigen::VectorXd, double, double, double, double> ((bp::arg("name"), bp::arg("robot"), bp::arg("framename"), 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::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")
......@@ -55,7 +55,6 @@ namespace tsid
.add_property("getForceGeneratorMatrix", bp::make_function(&ContactPointPythonVisitor::getForceGeneratorMatrix, bp::return_value_policy<bp::copy_const_reference>()))
.add_property("getForceRegularizationWeight", &ContactPoint::getForceRegularizationWeight, "return force reg weight")
.def("getNormalForce", &ContactPointPythonVisitor::getNormalForce, bp::arg("vec"))
.add_property("getMinNormalForce", &ContactPoint::getMinNormalForce)
.add_property("getMaxNormalForce", &ContactPoint::getMaxNormalForce)
......@@ -71,7 +70,6 @@ namespace tsid
.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("setRegularizationTaskWeight", &ContactPointPythonVisitor::setRegularizationTaskWeight, bp::args("double"))
.def("setReference", &ContactPointPythonVisitor::setReference, bp::args("SE3"))
.def("setForceReference", &ContactPointPythonVisitor::setForceReference, bp::args("f_vec"))
.def("setRegularizationTaskWeightVector", &ContactPointPythonVisitor::setRegularizationTaskWeightVector, bp::args("w_vec"))
......@@ -131,9 +129,6 @@ namespace tsid
static bool setMaxNormalForce (ContactPoint & self, const double maxNormalForce){
return self.setMaxNormalForce(maxNormalForce);
}
static bool setRegularizationTaskWeight (ContactPoint & self, const double w){
return self.setRegularizationTaskWeight(w);
}
static void setReference(ContactPoint & self, const se3::SE3 & ref){
self.setReference(ref);
}
......@@ -160,4 +155,4 @@ namespace tsid
}
#endif // ifndef __tsid_python_contact_hpp__
\ No newline at end of file
#endif // ifndef __tsid_python_contact_hpp__
......@@ -57,9 +57,10 @@ namespace tsid
.def("updateTaskWeight", &InvDynPythonVisitor::updateTaskWeight, bp::args("task_name", "weight"))
.def("addRigidContact", &InvDynPythonVisitor::addRigidContact6d, bp::args("contact"))
.def("addRigidContact", &InvDynPythonVisitor::addRigidContactPoint, bp::args("contact"))
.def("addRigidContact", &InvDynPythonVisitor::addRigidContactPointWithPriorityLevel, bp::args("contact", "priority_level"))
.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"))
......@@ -87,14 +88,20 @@ namespace tsid
static bool updateTaskWeight(T& self, const std::string & task_name, double weight){
return self.updateTaskWeight(task_name, weight);
}
static bool addRigidContact6d(T& self, contacts::Contact6d & contact){
static bool addRigidContact6dDeprecated(T& self, contacts::Contact6d & contact){
return self.addRigidContact(contact);
}
static bool addRigidContactPoint(T& self, contacts::ContactPoint & 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, const bool priority_level){
return self.addRigidContact(contact, priority_level);
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);
......@@ -140,4 +147,4 @@ namespace tsid
}
#endif // ifndef __tsid_python_HQPOutput_hpp__
\ No newline at end of file
#endif // ifndef __tsid_python_HQPOutput_hpp__
Subproject commit a921f1ead10e551804da70d6080a95dba1e673bd
Subproject commit 8e7bedfcbd8524c0401a58fd74edc07c3d4308d0
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
import commands
np.set_printoptions(precision=3, linewidth=200, suppress=True)
print("".center(100,'#'))
print(" Test Task Space Inverse Dynamics ".center(100, '#'))
print("".center(100,'#'), '\n')
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 = 30.0 # maximum 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 = 100.0 # proportional gain of contact constraint
kp_com = 100.0 # proportional gain of center of mass 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
N_SIMULATION = 1000 # number of time steps simulated
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'
......@@ -31,111 +41,133 @@ 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)
srdf = path + '/srdf/romeo_collision.srdf'
print("Creating RobotWrapper")
# for gepetto viewer .. but Fix me!!
# for gepetto viewer
robot_display = se3.RobotWrapper(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 &')
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)
print("Connect to gepetto server.")
cl = gepetto.corbaserver.Client()
gui = cl.gui
robot_display.initDisplay(loadModel=True)
q = np.matrix(np.zeros(robot.nq)).T
q[2] = 0.5
q[0] = 0.1
q[6] = 1.
#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 = np.matrix(np.zeros(robot.nv)).transpose()
# Create the main invdyn formulation object.
t = 0.0
invdyn = tsid.InverseDynamicsFormulationAccForce("tsid", robot, True)
invdyn.computeProblemData(t, q, v)
data = invdyn.data()
robot.computeAllTerms(data, q, v)
# Place the robot onto the ground.
id_fl_contact = robot_display.model.getFrameId('FL_contact')
q[2] -= robot.framePosition(data, id_fl_contact).translation[2, 0]
v = matlib.zeros(robot.nv).T
contact_frames = [
'BL_contact', 'BR_contact', 'FL_contact', 'FR_contact'
]
robot_display.displayCollisions(False)
robot_display.displayVisuals(True)
robot_display.display(q)
robot.computeAllTerms(data, q, v)
assert [robot.model().existFrame(name) for name in contact_frames]
# Add task for the COM.
com_ref = robot.com(data)
comTask = tsid.TaskComEquality("task-com", robot)
comTask.setKp(kp_com * np.matrix(np.ones(3)).transpose())
comTask.setKd(2.0 * np.sqrt(kp_com) * np.matrix(np.ones(3)).transpose())
invdyn.addMotionTask(comTask, w_com, 1, 0.0)
trajCom = tsid.TrajectoryEuclidianConstant("traj_com", com_ref)
comTask.setReference(trajCom.computeNext())
t = 0.0 # time
invdyn = tsid.InverseDynamicsFormulationAccForce("tsid", robot, False)
invdyn.computeProblemData(t, q, v)
data = invdyn.data()
# Add contact constraint for the point feets.
# HACK: Not taking the feet orientation into account for local frame right now.
# 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)
task_contacts = []
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)
for cframe in contact_frames:
contact = tsid.ContactPoint("contact_" + cframe, robot, cframe, contactNormal, mu, fMin, fMax, w_forceRef)
contact.setKp(kp_contact * np.matrix(np.ones(3)).transpose())
contact.setKd(2.0 * np.sqrt(kp_contact) * np.matrix(np.ones(3)).transpose())
contact.setReference(robot.framePosition(data, robot_display.model.getFrameId(cframe)))
contact.useLocalFrame(False)
invdyn.addRigidContact(contact, 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)
task_contacts.append(contact)
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)
# Add task to keep the robot at the current com position.
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.SolverHQuadProg("qp solver")
solver = tsid.SolverHQuadProgFast("qp solver")
solver.resize(invdyn.nVar, invdyn.nEq, invdyn.nIn)
print('COM start:', com_ref)
# for i in range(0, N_SIMULATION):
for i in range(0, N_SIMULATION):
HQPData = invdyn.computeProblemData(t, q, v)
sol = solver.solve(HQPData)
tau = invdyn.getActuatorForces(sol)
dv = invdyn.getAccelerations(sol)
lam = invdyn.getContactForces(sol)
com_pos = matlib.empty((3, N_SIMULATION))*nan
com_vel = matlib.empty((3, N_SIMULATION))*nan
com_acc = matlib.empty((3, N_SIMULATION))*nan
# Minimal integrator.
v_mean = v + 0.5*dt*dv
v += dt*dv
q = se3.integrate(robot.model(), q, dt*v_mean)
t += dt
com_pos_ref = matlib.empty((3, N_SIMULATION))*nan
com_vel_ref = matlib.empty((3, N_SIMULATION))*nan
com_acc_ref = matlib.empty((3, N_SIMULATION))*nan
com_acc_des = matlib.empty((3, N_SIMULATION))*nan # acc_des = acc_ref - Kp*pos_err - Kd*vel_err
com_ref[2] += 0.05 * dt
trajCom.setReference(com_ref)
comTask.setReference(trajCom.computeNext())
offset = robot.com(data) + np.matrix([0.0, 0.0, 0.0]).T
amp = np.matrix([0.0, 0.03, 0.05]).T
two_pi_f = 2*np.pi*np.matrix([0.0, 0.5, 0.7]).T
two_pi_f_amp = np.multiply(two_pi_f,amp)
two_pi_f_squared_amp = np.multiply(two_pi_f, two_pi_f_amp)
robot_display.display(q)
time.sleep(0.001)
robot.computeAllTerms(data, q, v)
com_final = robot.com(data)
print('COM ref:', com_ref)
print('COM final:', com_final)
print('COM error:', np.linalg.norm(com_final - com_ref))
for i in range(0, N_SIMULATION):
time_start = time.time()
sampleCom.pos(offset + np.multiply(amp, matlib.sin(two_pi_f*t)))
sampleCom.vel(np.multiply(two_pi_f_amp, matlib.cos(two_pi_f*t)))
sampleCom.acc(np.multiply(two_pi_f_squared_amp, -matlib.sin(two_pi_f*t)))
comTask.setReference(sampleCom)
samplePosture = trajPosture.computeNext()
postureTask.setReference(samplePosture)
HQPData = invdyn.computeProblemData(t, q, v)
if i == 0: HQPData.print_all()
sol = solver.solve(HQPData)
if(sol.status!=0):
print "[%d] QP problem could not be solved! Error code:"%(i), sol.status
break
tau = invdyn.getActuatorForces(sol)
dv = invdyn.getAccelerations(sol)
com_pos[:,i] = robot.com(invdyn.data())
com_vel[:,i] = robot.com_vel(invdyn.data())
com_acc[:,i] = comTask.getAcceleration(dv)
com_pos_ref[:,i] = sampleCom.pos()
com_vel_ref[:,i] = sampleCom.vel()
com_acc_ref[:,i] = sampleCom.acc()
com_acc_des[:,i] = comTask.getDesiredAcceleration
if i%PRINT_N == 0:
print "Time %.3f"%(t)
print "\tNormal forces: ",
for contact in contacts:
if invdyn.checkContact(contact.name, sol):
f = invdyn.getContactForce(contact.name, sol)
print "%4.1f"%(contact.getNormalForce(f)),
print "\n\ttracking err %s: %.3f"%(comTask.name.ljust(20,'.'), norm(comTask.position_error, 2))
print "\t||v||: %.3f\t ||dv||: %.3f"%(norm(v, 2), norm(dv))
v_mean = v + 0.5*dt*dv
v += dt*dv
q = se3.integrate(robot.model(), q, dt*v_mean)
t += dt
if i%DISPLAY_N == 0: robot_display.display(q)
time_spent = time.time() - time_start
if(time_spent < dt): time.sleep(dt-time_spent)
......@@ -18,6 +18,7 @@
#ifndef __invdyn_contact_6d_hpp__
#define __invdyn_contact_6d_hpp__
#include "tsid/deprecation.hpp"
#include "tsid/contacts/contact-base.hpp"
#include "tsid/tasks/task-se3-equality.hpp"
#include "tsid/math/constraint-inequality.hpp"
......@@ -45,6 +46,15 @@ namespace tsid
typedef se3::SE3 SE3;
Contact6d(const std::string & name,
RobotWrapper & robot,
const std::string & frameName,
ConstRefMatrix contactPoints,
ConstRefVector contactNormal,
const double frictionCoefficient,
const double minNormalForce,
const double maxNormalForce);
DEPRECATED Contact6d(const std::string & name,
RobotWrapper & robot,
const std::string & frameName,
ConstRefMatrix contactPoints,
......@@ -52,7 +62,7 @@ namespace tsid
const double frictionCoefficient,
const double minNormalForce,
const double maxNormalForce,
const double regularizationTaskWeight);
const double forceRegWeight);
/// Return the number of motion constraints
virtual unsigned int n_motion() const;
......@@ -81,7 +91,6 @@ namespace tsid
const ConstraintBase & getMotionConstraint() const;
const ConstraintInequality & getForceConstraint() const;
const ConstraintEquality & getForceRegularizationTask() const;
double getForceRegularizationWeight() const;
double getNormalForce(ConstRefVector f) const;
double getMinNormalForce() const;
......@@ -98,7 +107,6 @@ namespace tsid
bool setFrictionCoefficient(const double frictionCoefficient);
bool setMinNormalForce(const double minNormalForce);
bool setMaxNormalForce(const double maxNormalForce);
bool setRegularizationTaskWeight(const double w);
void setReference(const SE3 & ref);
void setForceReference(ConstRefVector & f_ref);
void setRegularizationTaskWeightVector(ConstRefVector & w);
......@@ -119,7 +127,6 @@ namespace tsid
double m_mu;
double m_fMin;
double m_fMax;
double m_regularizationTaskWeight;
Matrix m_forceGenMat;
};
}
......
......@@ -79,7 +79,6 @@ namespace tsid
virtual const ConstraintBase & getMotionConstraint() const = 0;
virtual const ConstraintInequality & getForceConstraint() const = 0;
virtual const ConstraintEquality & getForceRegularizationTask() const = 0;
virtual double getForceRegularizationWeight() const = 0;
virtual double getMinNormalForce() const = 0;
virtual double getMaxNormalForce() const = 0;
......
......@@ -50,8 +50,7 @@ namespace tsid
ConstRefVector contactNormal,
const double frictionCoefficient,
const double minNormalForce,
const double maxNormalForce,
const double regularizationTaskWeight);
const double maxNormalForce);
/// Return the number of motion constraints
virtual unsigned int n_motion() const;
......@@ -80,7 +79,7 @@ namespace tsid
const ConstraintBase & getMotionConstraint() const;
const ConstraintInequality & getForceConstraint() const;
const ConstraintEquality & getForceRegularizationTask() const;
double getForceRegularizationWeight() const;
double getMotionTaskWeight() const;
double getNormalForce(ConstRefVector f) const;
double getMinNormalForce() const;
......@@ -96,7 +95,7 @@ namespace tsid
bool setFrictionCoefficient(const double frictionCoefficient);
bool setMinNormalForce(const double minNormalForce);
bool setMaxNormalForce(const double maxNormalForce);
bool setRegularizationTaskWeight(const double w);
bool setMotionTaskWeight(const double w);
void setReference(const SE3 & ref);
void setForceReference(ConstRefVector & f_ref);
void setRegularizationTaskWeightVector(ConstRefVector & w);
......@@ -128,6 +127,7 @@ namespace tsid
double m_fMin;
double m_fMax;
double m_regularizationTaskWeight;
double m_motionTaskWeight;
Matrix m_forceGenMat;
};
}
......
......@@ -111,7 +111,10 @@ namespace tsid
bool updateTaskWeight(const std::string & task_name,
double weight);
bool addRigidContact(ContactBase & contact, unsigned int motionPriorityLevel=0);
bool addRigidContact(ContactBase & contact, double force_regularization_weight,
double motion_weight=1.0, unsigned int motion_priority_level=0);
DEPRECATED bool addRigidContact(ContactBase & contact);
bool removeTask(const std::string & taskName,
double transition_duration=0.0);
......
......@@ -18,6 +18,7 @@
#ifndef __invdyn_inverse_dynamics_formulation_base_hpp__
#define __invdyn_inverse_dynamics_formulation_base_hpp__
#include "tsid/deprecation.hpp"
#include "tsid/math/fwd.hpp"
#include "tsid/robots/robot-wrapper.hpp"
#include "tsid/tasks/task-actuation.hpp"
......@@ -81,7 +82,19 @@ namespace tsid
virtual bool updateTaskWeight(const std::string & task_name,
double weight) = 0;
virtual bool addRigidContact(ContactBase & contact, unsigned int motionPriorityLevel=0) = 0;
/**
* @brief Add a rigid contact constraint to the model, introducing the associated reaction forces
* as problem variables.
* @param contact The contact constraint to add
* @param force_regularization_weight The weight of the force regularization task
* @param motion_weight The weight of the motion task (e.g., zero acceleration of contact points)
* @param motion_priority_level Priority level of the motion task
* @return True if everything went fine, false otherwise
*/
virtual bool addRigidContact(ContactBase & contact, double force_regularization_weight,
double motion_weight=1.0, unsigned int motion_priority_level=0) = 0;
DEPRECATED virtual bool addRigidContact(ContactBase & contact);
virtual bool removeTask(const std::string & taskName,
double transition_duration=0.0) = 0;
......
import pinocchio as se3
import tsid
import numpy as np
import copy
print ""
print "Test Contact"
print ""
tol = 1e-5
import os
filename = str(os.path.dirname(os.path.abspath(__file__)))
path = filename + '/../models/romeo'
urdf = path + '/urdf/romeo.urdf'
vector = se3.StdVec_StdString()
vector.extend(item for item in path)
robot = tsid.RobotWrapper(urdf, vector, se3.JointModelFreeFlyer(), False)
model = robot.model()
data = robot.data()
mu = 0.3
fMin = 10.0
fMax = 1000.0
frameName = "RAnkleRoll"
contactNormal = np.matrix(np.zeros(3)).transpose()
contactNormal[2] = 1.0
contact = tsid.ContactPoint("contactPoint", robot, frameName, contactNormal, mu, fMin, fMax)
assert contact.n_motion == 3
assert contact.n_force == 3
Kp = np.matrix(np.ones(3)).transpose()
Kd = 2*Kp
contact.setKp(Kp)
contact.setKd(Kd)
assert np.linalg.norm(contact