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

Merge pull request #50 from nmansard/topic/cof

Add residual for "center of friction" 
parents 238dc7ee 37c5f411
......@@ -64,6 +64,14 @@ struct ResidualDataCenterOfPressureTpl;
typedef ResidualModelCenterOfPressureTpl<double> ResidualModelCenterOfPressure;
typedef ResidualDataCenterOfPressureTpl<double> ResidualDataCenterOfPressure;
// Cost CenterOfFriction
template <typename Scalar>
class ResidualModelCenterOfFrictionTpl;
template <typename Scalar>
struct ResidualDataCenterOfFrictionTpl;
typedef ResidualModelCenterOfFrictionTpl<double> ResidualModelCenterOfFriction;
typedef ResidualDataCenterOfFrictionTpl<double> ResidualDataCenterOfFriction;
// Cost velocity collision
template <typename Scalar>
class ResidualModelVelCollisionTpl;
......
///////////////////////////////////////////////////////////////////////////////
// BSD 3-Clause License
//
// Copyright (C) 2022, LAAS-CNRS
// Copyright note valid unless otherwise stated in individual files.
// All rights reserved.
///////////////////////////////////////////////////////////////////////////////
#ifndef SOBEC_RESIDUAL_CENTER_OF_FRICTION_HPP_
#define SOBEC_RESIDUAL_CENTER_OF_FRICTION_HPP_
#include <crocoddyl/core/residual-base.hpp>
#include <crocoddyl/core/utils/exception.hpp>
#include <crocoddyl/multibody/data/multibody.hpp>
#include <crocoddyl/multibody/fwd.hpp>
#include <crocoddyl/multibody/states/multibody.hpp>
#include <pinocchio/algorithm/jacobian.hpp>
#include <pinocchio/multibody/fwd.hpp>
#include <pinocchio/spatial/motion.hpp>
#include "crocoddyl/multibody/contacts/contact-6d.hpp"
#include "crocoddyl/multibody/contacts/multiple-contacts.hpp"
#include "crocoddyl/multibody/data/contacts.hpp"
#include "sobec/fwd.hpp"
namespace sobec {
using namespace crocoddyl;
/**
* @brief Center of friction residual
*
* residual = [ tau_y/f_z, -tau_x/fx ]
*
* As described in `ResidualModelAbstractTpl()`, the residual value and its
* Jacobians are calculated by `calc` and `calcDiff`, respectively.
*
* \sa `ResidualModelAbstractTpl`, `calc()`, `calcDiff()`, `createData()`
*/
template <typename _Scalar>
class ResidualModelCenterOfFrictionTpl
: public ResidualModelAbstractTpl<_Scalar> {
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
typedef _Scalar Scalar;
typedef MathBaseTpl<Scalar> MathBase;
typedef ResidualModelAbstractTpl<Scalar> Base;
typedef ResidualDataCenterOfFrictionTpl<Scalar> Data;
typedef ResidualDataAbstractTpl<Scalar> ResidualDataAbstract;
typedef StateMultibodyTpl<Scalar> StateMultibody;
typedef DataCollectorAbstractTpl<Scalar> DataCollectorAbstract;
typedef typename MathBase::VectorXs VectorXs;
typedef typename MathBase::Vector3s Vector3s;
typedef typename MathBase::MatrixXs MatrixXs;
typedef typename pinocchio::ForceTpl<Scalar> Force;
/**
* @brief Initialize the residual model
*
* @param[in] state State of the multibody system
* @param[in] nu Dimension of the control vector
* @param[in] geom_model Pinocchio geometry model containing the collision
* pair
* @param[in] pair_id Index of the collision pair in the geometry model
* @param[in] joint_id Index of the nearest joint on which the collision
* link is attached
*/
ResidualModelCenterOfFrictionTpl(boost::shared_ptr<StateMultibody> state,
const pinocchio::FrameIndex contact_id,
const std::size_t nu);
virtual ~ResidualModelCenterOfFrictionTpl();
/**
* @brief Compute the center of friction.
*
* @param[in] data residual data
* @param[in] x State point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$
* @param[in] u Control input \f$\mathbf{u}\in\mathbb{R}^{nu}\f$
*/
virtual void calc(const boost::shared_ptr<ResidualDataAbstract> &data,
const Eigen::Ref<const VectorXs> &x,
const Eigen::Ref<const VectorXs> &u);
/**
* @brief Compute the derivatives of residual
*
* @param[in] data residual data
* @param[in] x State point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$
* @param[in] u Control input \f$\mathbf{u}\in\mathbb{R}^{nu}\f$
*/
virtual void calcDiff(const boost::shared_ptr<ResidualDataAbstract> &data,
const Eigen::Ref<const VectorXs> &x,
const Eigen::Ref<const VectorXs> &u);
virtual boost::shared_ptr<ResidualDataAbstract> createData(
DataCollectorAbstract *const data);
/**
* @brief Return the reference contact id
*/
pinocchio::FrameIndex get_contact_id() const { return contact_id_; }
/** @brief Set the reference contact id. */
void set_contact_id(const pinocchio::FrameIndex id) { contact_id_ = id; }
protected:
using Base::nu_;
using Base::state_;
using Base::unone_;
private:
pinocchio::FrameIndex contact_id_;
};
template <typename _Scalar>
struct ResidualDataCenterOfFrictionTpl
: public ResidualDataAbstractTpl<_Scalar> {
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
typedef _Scalar Scalar;
typedef MathBaseTpl<Scalar> MathBase;
typedef ResidualDataAbstractTpl<Scalar> Base;
typedef StateMultibodyTpl<Scalar> StateMultibody;
typedef DataCollectorAbstractTpl<Scalar> DataCollectorAbstract;
typedef typename MathBase::Matrix6xs Matrix6xs;
template <template <typename Scalar> class Model>
ResidualDataCenterOfFrictionTpl(Model<Scalar> *const model,
DataCollectorAbstract *const data)
: Base(model, data) {
// Check that proper shared data has been passed
DataCollectorContactTpl<Scalar> *d =
dynamic_cast<DataCollectorContactTpl<Scalar> *>(this->shared);
if (d == NULL) {
throw_pretty(
"Invalid argument: the shared data should be derived from "
"DataCollectorContact");
}
const pinocchio::FrameIndex id = model->get_contact_id();
const boost::shared_ptr<StateMultibody> &state =
boost::static_pointer_cast<StateMultibody>(model->get_state());
std::string frame_name = state->get_pinocchio()->frames[id].name;
bool found_contact = false;
for (auto &it : d->contacts->contacts) {
if (it.second->frame == id) {
ContactData6DTpl<Scalar> *d6d =
dynamic_cast<ContactData6DTpl<Scalar> *>(it.second.get());
if (d6d != NULL) {
found_contact = true;
this->contact = it.second;
break;
}
throw_pretty(
"Domain error: there isn't defined at least a 6d contact for " +
frame_name);
}
}
if (!found_contact) {
throw_pretty("Domain error: there isn't defined contact data for " +
frame_name);
}
}
boost::shared_ptr<ForceDataAbstractTpl<Scalar> > contact;
using Base::r;
using Base::Ru;
using Base::Rx;
using Base::shared;
};
} // namespace sobec
/* --- Details -------------------------------------------------------------- */
/* --- Details -------------------------------------------------------------- */
/* --- Details -------------------------------------------------------------- */
#include "sobec/residual-center-of-friction.hxx"
#endif // SOBEC_RESIDUAL_CENTER_OF_FRICTION_HPP_
///////////////////////////////////////////////////////////////////////////////
// BSD 3-Clause License
//
// Copyright (C) 2021, LAAS-CNRS, University of Edinburgh, INRIA
// Copyright note valid unless otherwise stated in individual files.
// All rights reserved.
///////////////////////////////////////////////////////////////////////////////
#include <crocoddyl/core/utils/exception.hpp>
#include <pinocchio/algorithm/frames-derivatives.hpp>
#include <pinocchio/algorithm/frames.hpp>
#include <pinocchio/algorithm/kinematics-derivatives.hpp>
#include "sobec/residual-center-of-friction.hpp"
namespace sobec {
using namespace crocoddyl;
template <typename Scalar>
ResidualModelCenterOfFrictionTpl<Scalar>::ResidualModelCenterOfFrictionTpl(
boost::shared_ptr<StateMultibody> state,
const pinocchio::FrameIndex contact_id, const std::size_t nu)
: Base(state, 2, nu, true, true, true), contact_id_(contact_id) {}
template <typename Scalar>
ResidualModelCenterOfFrictionTpl<Scalar>::~ResidualModelCenterOfFrictionTpl() {}
template <typename Scalar>
void ResidualModelCenterOfFrictionTpl<Scalar>::calc(
const boost::shared_ptr<ResidualDataAbstract> &data,
const Eigen::Ref<const VectorXs> & /*x*/,
const Eigen::Ref<const VectorXs> &) {
Data *d = static_cast<Data *>(data.get());
Force f = d->contact->jMf.actInv(d->contact->f);
data->r[0] = f.linear()[1] / f.linear()[2];
data->r[1] = -f.linear()[0] / f.linear()[2];
}
template <typename Scalar>
void ResidualModelCenterOfFrictionTpl<Scalar>::calcDiff(
const boost::shared_ptr<ResidualDataAbstract> &data,
const Eigen::Ref<const VectorXs> &, const Eigen::Ref<const VectorXs> &) {
Data *d = static_cast<Data *>(data.get());
Force f = d->contact->jMf.actInv(d->contact->f);
const MatrixXs &df_dx = d->contact->df_dx;
const MatrixXs &df_du = d->contact->df_du;
// r = tau/f
// r'= tau'/f - tau/f^2 f' = (tau'-cop.f')/f
data->Rx.row(0) = df_dx.row(1);
data->Rx.row(1) = -df_dx.row(0);
data->Rx.row(0) -= data->r[0] * df_dx.row(2);
data->Rx.row(1) -= data->r[1] * df_dx.row(2);
data->Rx /= f.linear()[2];
data->Ru.row(0) = df_du.row(1);
data->Ru.row(1) = -df_du.row(0);
data->Ru.row(0) -= data->r[0] * df_du.row(2);
data->Ru.row(1) -= data->r[1] * df_du.row(2);
data->Ru /= f.linear()[2];
}
template <typename Scalar>
boost::shared_ptr<ResidualDataAbstractTpl<Scalar> >
ResidualModelCenterOfFrictionTpl<Scalar>::createData(
DataCollectorAbstract *const data) {
return boost::allocate_shared<Data>(Eigen::aligned_allocator<Data>(), this,
data);
}
} // namespace sobec
......@@ -2,12 +2,16 @@ import sobec.walk.params as swparams
import numpy as np
def roundToEven(xfloat):
return 2 * int(np.round(xfloat / 2 - 0.5001)) + 1
class WalkParams(swparams.WalkParams):
DT = 0.015
Tstart = int(0.3 / DT)
Tsingle = int(0.8 / DT) # 60
# I prefer an even number for Tdouble
Tdouble = 2 * int(np.round(0.11 / DT / 2 - 0.75)) + 1 # 11
Tdouble = roundToEven(0.11 / DT) # 11
Tend = int(0.3 / DT)
Tmpc = int(1.4 / DT) # 1.6
vcomRef = np.array([0.05, 0, 0])
......@@ -23,7 +27,7 @@ class PushParams(swparams.WalkParams):
Tstart = int(0.3 / DT)
Tsingle = int(0.6 / DT) # 60
# I prefer an even number for Tdouble
Tdouble = 2 * int(np.round(0.11 / DT / 2 - 0.75)) + 1 # 11
Tdouble = roundToEven(0.11 / DT) # 11
Tend = int(0.3 / DT)
Tmpc = int(1.4 / DT) # 1.6
......@@ -41,35 +45,36 @@ class StandParams(swparams.WalkParams):
"""MPC Params with not single support, hence standing straight"""
DT = 0.01
Tsimu = 500
Tsimu = 200
Tstart = 50
Tsingle = 1 # int(0.8 / DT)
# I prefer an even number for Tdouble
Tdouble = 51 # 2 * int(np.round(0.11 / DT / 2 - 0.75)) + 1 # 11
Tdouble = roundToEven(71)
Tend = 50
Tmpc = 60
Tmpc = 80
transitionDuration = (Tdouble - 1) // 2
vcomRef = np.array([0, 0, 0])
vcomImportance = np.array([0.0, 0, 1])
vcomWeight = 2
vcomImportance = np.array([0.0, 3, 1])
vcomWeight = 20
refStateWeight = 1e-1
forceImportance = np.array([1, 1, 0.1, 10, 10, 2])
coneAxisWeight = 5e-5 # 2e-4
copWeight = 2 # 2
refForceWeight = 20 # 10
copWeight = 3 # 2
centerOfFrictionWeight = 1 # 2
refForceWeight = 10 # 10
minimalNormalForce = 50
withNormalForceBoundOnly = True
conePenaltyWeight = 1
conePenaltyWeight = 0
refTorqueWeight = 0
comWeight = 10 # 20
verticalFootVelWeight = 0
comWeight = 1 # 20
verticalFootVelWeight = 10
flyHighWeight = 0
groundColWeight = 0
conePenaltyWeight = 0
feetCollisionWeight = 0
impactAltitudeWeight = 2000 # 20000
impactVelocityWeight = 200 # 10000
......
......@@ -193,6 +193,56 @@ mpc.solver.setCallbacks(
]
)
"""
import matplotlib.pylab as plt # noqa: E402,F401
import utils.walk_plotter as walk_plotter # noqa: E402
sol = ocp.Solution(robot, ddp)
ocpplotter = walk_plotter.WalkPlotter(robot.model, robot.contactIds)
ocpplotter.setData(contactPattern, sol.xs, sol.us, sol.fs0)
target = problem.terminalModel.differential.costs.costs[
"stateReg"
].cost.residual.reference
forceRef = [
walk_plotter.getReferenceForcesFromProblemModels(problem, cid)
for cid in robot.contactIds
]
forceRef = [np.concatenate(fs) for fs in zip(*forceRef)]
wp=walkParams
frefocp = np.hstack([walk_plotter.getReferenceForcesFromProblemModels(ddp.problem,cid)
for cid in robot.contactIds ])
Tcycle = (wp.Tdouble+wp.Tsingle)*2
plt.figure()
plt.plot(frefocp[:,[2,8]])
plt.plot([wp.Tstart,wp.Tstart],[0,1000],'r')
wp=walkParams
plt.plot([wp.Tstart,wp.Tstart],[0,1000],'r')
plt.plot([wp.Tstart+wp.Tdouble]*2,[0,1000],'r')
plt.plot([wp.Tstart+wp.Tdouble+wp.Tsingle]*2,[0,1000],'r')
plt.plot([wp.Tstart+wp.Tdouble+wp.Tsingle+wp.Tdouble]*2,[0,1000],'r')
plt.plot([wp.Tstart+wp.Tdouble+wp.Tsingle+wp.Tdouble+wp.Tsingle]*2,[0,1000],'r')
plt.plot([wp.Tstart+wp.Tdouble+wp.Tsingle+wp.Tdouble+wp.Tsingle+wp.Tdouble]*2,[0,1000],'r')
plt.plot([wp.Tstart+wp.Tdouble+wp.Tsingle+wp.Tdouble+wp.Tsingle+wp.Tdouble+wp.Tend]*2,[0,1000],'r')
plt.plot([wp.Tstart+wp.Tdouble//2]*2,[0,1000],'k')
plt.plot([wp.Tstart+wp.Tdouble//2+Tcycle]*2,[0,1000],'k')
hf = [ f for f in frefocp[:wp.Tmpc,:]]
for t in range(1000):
Tinit = wp.Tstart+wp.Tdouble//2;
tlast = Tinit + ((t + wp.Tmpc - Tinit) % Tcycle);
hf.append( frefocp[tlast,:] )
hf = np.array(hf)
plt.figure()
plt.plot(hf[:,[2,8]])
stophere
"""
# #####################################################################################
# ### VIZ #############################################################################
# #####################################################################################
......@@ -214,6 +264,7 @@ except (ImportError, AttributeError):
hx = [simu.getState()]
hu = []
hus = []
hxs = []
hf = []
hfref = []
......@@ -271,6 +322,7 @@ for s in range(walkParams.Tsimu): # int(20.0 / walkParams.DT)):
# ### LOG DATA
hxs.append(np.array(mpc.solver.xs))
hus.append(np.array(mpc.solver.us))
contactsm0 = mpc.problem.runningModels[0].differential.contacts.contacts
contactsd0 = mpc.problem.runningDatas[0].differential.multibody.contacts.contacts
fs = []
......@@ -359,8 +411,9 @@ plotter.plotFeet()
plotter.plotFootCollision(walkParams.footMinimalDistance, 50)
plotter.plotJointTorques()
plotter.plotForces(hfref)
# mpcplotter = walk_plotter.WalkRecedingPlotter(robot.model, robot.contactIds, hxs)
# mpcplotter.plotFeet()
plotter.plotAllForces()
plotter.plotTimeCop()
plotter.plotTimeCof()
print("Run ```plt.ion(); plt.show()``` to display the plots.")
......
......@@ -74,6 +74,23 @@ class WalkPlotter:
cop = [[t, [f[4] / f[2], -f[3] / f[2]]] for (t, f) in ftraj]
plt.plot([t for t, p in cop], [p for t, p in cop], ".")
def plotTimeCof(self):
plt.figure("cof time local")
for ifig, cid in enumerate(self.contactIds):
plt.subplot(len(self.contactIds), 1, ifig + 1)
# ftraj = [
# [t, f[6 * ifig : 6 * ifig + 6]]
# for t, (f, p) in enumerate(zip(self.fs, self.contactPattern))
# if cid in patternToId(p)
# ]
ftraj = [
[t, f[6 * ifig : 6 * ifig + 6]]
for t, (f, p) in enumerate(zip(self.fs, self.contactPattern))
if p[ifig]
]
cof = [[t, [f[1] / f[2], -f[0] / f[2]]] for (t, f) in ftraj]
plt.plot([t for t, p in cof], [p for t, p in cof], ".")
def plotCopAndFeet(self, footSize, ARENA_SIZE=0.6):
# Cop of each foot in x-vs-y (with limits)
plt.figure(figsize=(12, 6))
......@@ -126,6 +143,29 @@ class WalkPlotter:
plt.plot(frefplot[:, 8])
plt.xlabel(self.model.frames[self.contactIds[1]].name)
def plotAllForces(self):
# Forces and reference forces wrt time
fig, axs = plt.subplots(5, 1, sharex=True, figsize=(6, 8))
fs0plot = np.array(self.fs)
hp1 = axs[0].plot(fs0plot[:, [0, 1]])
hp2 = axs[0].plot(fs0plot[:, [6, 7]])
axs[0].set_xlabel("lat. f")
axs[0].legend(hp1 + hp2, ["xl", "yl", "xr", "yl"])
hp1 = axs[1].plot(fs0plot[:, [2, 8]])
axs[1].set_xlabel("norm f")
axs[1].legend(hp1 + hp2, ["zl", "zr"])
hp1 = axs[2].plot(fs0plot[:, [3, 4]])
hp2 = axs[2].plot(fs0plot[:, [9, 10]])
axs[2].set_xlabel("lat. tau")
axs[2].legend(hp1 + hp2, ["xl", "yl", "xr", "yl"])
hp1 = axs[3].plot(fs0plot[:, [5, 11]])
axs[3].set_xlabel("norm. tau")
axs[3].legend(hp1 + hp2, ["zl", "zr"])
def plotCom(self, com0):
# COM position and velocity (x+y separated from z)
plt.figure("com", figsize=(6, 8))
......
......@@ -3,6 +3,7 @@ set(${PY_NAME}_SOURCES
residual-com-velocity.cpp
residual-vel-collision.cpp
residual-cop.cpp
residual-center-of-friction.cpp
residual-feet-collision.cpp
residual-fly-high.cpp
activation-quad-ref.cpp
......
///////////////////////////////////////////////////////////////////////////////
// BSD 3-Clause License
//
// Copyright (C) 2021, University of Edinburgh, LAAS-CNRS, INRIA
// Copyright note valid unless otherwise stated in individual files.
// All rights reserved.
///////////////////////////////////////////////////////////////////////////////
#include "sobec/residual-center-of-friction.hpp"
#include <boost/python.hpp>
#include <boost/python/enum.hpp>
#include <eigenpy/eigenpy.hpp>
#include <pinocchio/multibody/fwd.hpp> // Must be included first!
#include "sobec/fwd.hpp"
namespace sobec {
namespace python {
using namespace crocoddyl;
namespace bp = boost::python;
void exposeResidualCenterOfFriction() {
bp::register_ptr_to_python<
boost::shared_ptr<ResidualModelCenterOfFriction> >();
bp::class_<ResidualModelCenterOfFriction, bp::bases<ResidualModelAbstract> >(
"ResidualModelCenterOfFriction",
bp::init<boost::shared_ptr<StateMultibody>, pinocchio::FrameIndex,
std::size_t>(
bp::args("self", "state", "contact_id", "nu"),
"Initialize the residual model r(x,u)=center of friction.\n\n"
":param state: state of the multibody system\n"
":param contact_id: reference contact frame\n"
":param nu: dimension of control vector"))
.def<void (ResidualModelCenterOfFriction::*)(
const boost::shared_ptr<ResidualDataAbstract>&,
const Eigen::Ref<const Eigen::VectorXd>&,
const Eigen::Ref<const Eigen::VectorXd>&)>(
"calc", &ResidualModelCenterOfFriction::calc,
bp::args("self", "data", "x", "u"),
"Compute the vel collision residual.\n\n"
":param data: residual data\n"
":param x: time-discrete state vector\n"
":param u: time-discrete control input")
.def<void (ResidualModelCenterOfFriction::*)(
const boost::shared_ptr<ResidualDataAbstract>&,
const Eigen::Ref<const Eigen::VectorXd>&)>(
"calc", &ResidualModelAbstract::calc, bp::args("self", "data", "x"))
.def<void (ResidualModelCenterOfFriction::*)(
const boost::shared_ptr<ResidualDataAbstract>&,
const Eigen::Ref<const Eigen::VectorXd>&,
const Eigen::Ref<const Eigen::VectorXd>&)>(
"calcDiff", &ResidualModelCenterOfFriction::calcDiff,
bp::args("self", "data", "x", "u"),
"Compute the Jacobians of the vel collision residual.\n\n"
"It assumes that calc has been run first.\n"
":param data: action data\n"
":param x: time-discrete state vector\n"
":param u: time-discrete control input\n")
.def<void (ResidualModelCenterOfFriction::*)(
const boost::shared_ptr<ResidualDataAbstract>&,
const Eigen::Ref<const Eigen::VectorXd>&)>(
"calcDiff", &ResidualModelAbstract::calcDiff,
bp::args("self", "data", "x"))
.def("createData", &ResidualModelCenterOfFriction::createData,
bp::with_custodian_and_ward_postcall<0, 2>(),
bp::args("self", "data"),
"Create the residual data.\n\n"
":param data: shared data\n"
":return residual data.")
.add_property(
"contact_id", &ResidualModelCenterOfFriction::get_contact_id,
&ResidualModelCenterOfFriction::set_contact_id, "Contact frame ID");
bp::register_ptr_to_python<
boost::shared_ptr<ResidualDataCenterOfFriction> >();
bp::class_<ResidualDataCenterOfFriction, bp::bases<ResidualDataAbstract> >(
"ResidualDataCenterOfFriction", "Data for vel collision residual.\n\n",
bp::init<ResidualModelCenterOfFriction*, DataCollectorAbstract*>(
bp::args("self", "model", "data"),
"Create Center of friction residual data.\n\n"
":param model: residual model\n"
":param data: shared data")[bp::with_custodian_and_ward<
1, 2, bp::with_custodian_and_ward<1, 3> >()]);
}
} // namespace python
} // namespace sobec
......@@ -94,6 +94,18 @@ def buildRunningModels(robotWrapper, contactPattern, params):
"%s_cop" % robot.model.frames[cid].name, copCost, p.copWeight
)
cofResidual = sobec.ResidualModelCenterOfPressure(state, cid, actuation.nu)
cofAct = croc.ActivationModelWeightedQuad(
np.array([1.0 / p.footSize**2] * 2)
)
cofCost = croc.CostModelResidual(state, cofAct, cofResidual)
if p.centerOfFrictionWeight > 0:
costs.addCost(
"%s_centeroffriction" % robot.model.frames[cid].name,
cofCost,