Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

[ExternalForcesAwareQP] Give the possibility to make the QP aware of external forces #452

Open
wants to merge 5 commits into
base: master
Choose a base branch
from
Open
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
53 changes: 53 additions & 0 deletions include/mc_rbdyn/ExternalTorqueSensor.h
Original file line number Diff line number Diff line change
@@ -0,0 +1,53 @@
/*
* Copyright 2015-2023 CNRS-UM LIRMM, CNRS-AIST JRL
*/

#pragma once

#include <mc_rbdyn/Device.h>

namespace mc_rbdyn
{

struct Robot;

/** This struct is intended to hold static information about an external torques sensor
* and the current reading of said sensor.
*/
struct MC_RBDYN_DLLAPI ExternalTorqueSensor : public Device
{
public:
/** Default constructor, this does not represent a valid external torques sensor */
ExternalTorqueSensor();

/** Construct a valid external torques sensor based on static information, this
* force sensor can then be used to provide sensing information to the robot.
*
* @param name Name of the sensor
*
* @param size Number of DoF
*
*/
ExternalTorqueSensor(const std::string & name, const int & size);

ExternalTorqueSensor(const ExternalTorqueSensor & ets);

ExternalTorqueSensor & operator=(const ExternalTorqueSensor & ets);

ExternalTorqueSensor(ExternalTorqueSensor &&) = default;

/** Destructor */
~ExternalTorqueSensor() noexcept override;

inline const Eigen::VectorXd & torques() const { return externalJointTorques_; }

inline void torques(Eigen::VectorXd & torques) { externalJointTorques_ = torques; }

DevicePtr clone() const override;

private:
Eigen::VectorXd externalJointTorques_;
int size_;
};

} // namespace mc_rbdyn
53 changes: 53 additions & 0 deletions include/mc_rbdyn/VirtualTorqueSensor.h
Original file line number Diff line number Diff line change
@@ -0,0 +1,53 @@
/*
* Copyright 2015-2023 CNRS-UM LIRMM, CNRS-AIST JRL
*/

#pragma once

#include <mc_rbdyn/Device.h>

namespace mc_rbdyn
{

struct Robot;

/** This struct is intended to hold static information about an external torques sensor
* and the current reading of said sensor.
*/
struct MC_RBDYN_DLLAPI VirtualTorqueSensor : public Device
{
public:
/** Default constructor, this does not represent a valid external torques sensor */
VirtualTorqueSensor();

/** Construct a valid external torques sensor based on static information, this
* force sensor can then be used to provide sensing information to the robot.
*
* @param name Name of the sensor
*
* @param size Number of DoF
*
*/
VirtualTorqueSensor(const std::string & name, const int & size);

VirtualTorqueSensor(const VirtualTorqueSensor & ets);

VirtualTorqueSensor & operator=(const VirtualTorqueSensor & ets);

VirtualTorqueSensor(VirtualTorqueSensor &&) = default;

/** Destructor */
~VirtualTorqueSensor() noexcept override;

inline const Eigen::VectorXd & torques() const { return virtualJointTorques_; }

inline void torques(Eigen::VectorXd & torques) { virtualJointTorques_ = torques; }

DevicePtr clone() const override;

private:
Eigen::VectorXd virtualJointTorques_;
int size_;
};

} // namespace mc_rbdyn
9 changes: 7 additions & 2 deletions include/mc_solver/DynamicsConstraint.h
Original file line number Diff line number Diff line change
Expand Up @@ -29,7 +29,11 @@ struct MC_SOLVER_DLLAPI DynamicsConstraint : public KinematicsConstraint
* \param timeStep Solver timestep
* \param infTorque If true, ignore the torque limits set in the robot model
*/
DynamicsConstraint(const mc_rbdyn::Robots & robots, unsigned int robotIndex, double timeStep, bool infTorque = false);
DynamicsConstraint(const mc_rbdyn::Robots & robots,
unsigned int robotIndex,
double timeStep,
bool infTorque = false,
bool compensateExternalForces = false);

/** Constructor
* Builds a damped joint limits constraint and a motion constr depending on
Expand All @@ -48,7 +52,8 @@ struct MC_SOLVER_DLLAPI DynamicsConstraint : public KinematicsConstraint
double timeStep,
const std::array<double, 3> & damper,
double velocityPercent = 1.0,
bool infTorque = false);
bool infTorque = false,
bool compensateExternalForces = false);

/** Returns the tasks::qp::MotionConstr
*
Expand Down
76 changes: 76 additions & 0 deletions include/mc_tasks/CompliantEndEffectorTask.h
Original file line number Diff line number Diff line change
@@ -0,0 +1,76 @@
/*
* Copyright 2015-2019 CNRS-UM LIRMM, CNRS-AIST JRL
*/

#pragma once

#include <mc_tasks/EndEffectorTask.h>
#include <RBDyn/MultiBody.h>
#include <Eigen/src/Core/Matrix.h>

namespace mc_tasks
{

/*! \brief Controls an end-effector
*
* This task is a thin wrapper around the appropriate tasks in Tasks.
* The task objective is given in the world frame. For relative control
* see mc_tasks::RelativeCompliantEndEffectorTask
*/
struct MC_TASKS_DLLAPI CompliantEndEffectorTask : public EndEffectorTask
{
public:
/*! \brief Constructor
*
* \param bodyName Name of the body to control
*
* \param robots Robots controlled by this task
*
* \param robotIndex Index of the robot controlled by this task
*
* \param stiffness Task stiffness
*
* \param weight Task weight
*
*/
CompliantEndEffectorTask(const std::string & bodyName,
const mc_rbdyn::Robots & robots,
unsigned int robotIndex,
double stiffness,
double weight);

/** Change acceleration
*
* \p refAccel Should be of size 6
*/
void refAccel(const Eigen::Vector6d & refAccel) noexcept;

// Set the compliant behavior of the task
void makeCompliant(bool compliance);
void setComplianceVector(Eigen::Vector6d gamma);

// Get compliance state of the task
bool isCompliant(void);
Eigen::Vector6d getComplianceVector(void);

protected:
void addToSolver(mc_solver::QPSolver & solver);

void update(mc_solver::QPSolver & solver);

void addToGUI(mc_rtc::gui::StateBuilder & gui);

Eigen::MatrixXd compliant_matrix_;

mc_tvm::Robot * tvm_robot_;

unsigned int rIdx_;

std::string bodyName_;

rbd::Jacobian * jac_;

Eigen::Vector6d refAccel_;
};

} // namespace mc_tasks
40 changes: 40 additions & 0 deletions include/mc_tasks/CompliantPostureTask.h
Original file line number Diff line number Diff line change
@@ -0,0 +1,40 @@
/*
* Copyright 2015-2022 CNRS-UM LIRMM, CNRS-AIST JRL
*/

#pragma once

#include <mc_tasks/PostureTask.h>

namespace mc_tasks
{

struct MC_TASKS_DLLAPI CompliantPostureTask : public PostureTask
{
public:
CompliantPostureTask(const mc_solver::QPSolver & solver, unsigned int rIndex, double stiffness, double weight);

/** Change reference acceleration
*
* \p refAccel Should be of size nrDof
*/
void refAccel(const Eigen::VectorXd & refAccel) noexcept;

// Set task to be compliant or not
void makeCompliant(bool compliance);
// Get compliance state of the task
bool isCompliant(void);

protected:
void update(mc_solver::QPSolver & solver);

void addToGUI(mc_rtc::gui::StateBuilder & gui);

bool isCompliant_;

mc_tvm::Robot & tvm_robot_;

Eigen::VectorXd refAccel_;
};

} // namespace mc_tasks
5 changes: 4 additions & 1 deletion include/mc_tvm/DynamicFunction.h
Original file line number Diff line number Diff line change
Expand Up @@ -14,6 +14,8 @@

#include <SpaceVecAlg/SpaceVecAlg>

#include <mc_rbdyn/VirtualTorqueSensor.h>

namespace mc_tvm
{

Expand All @@ -36,7 +38,7 @@ struct MC_TVM_DLLAPI DynamicFunction : public tvm::function::abstract::LinearFun
SET_UPDATES(DynamicFunction, Jacobian, B)

/** Construct the equation of motion for a given robot */
DynamicFunction(const mc_rbdyn::Robot & robot);
DynamicFunction(const mc_rbdyn::Robot & robot, bool compensateExternalForces = false);

/** Add a contact to the function
*
Expand Down Expand Up @@ -73,6 +75,7 @@ struct MC_TVM_DLLAPI DynamicFunction : public tvm::function::abstract::LinearFun
void updateb();

const mc_rbdyn::Robot & robot_;
const bool compensateExternalForces_;

/** Holds data for the force part of the motion equation */
struct ForceContact
Expand Down
19 changes: 17 additions & 2 deletions include/mc_tvm/Robot.h
Original file line number Diff line number Diff line change
Expand Up @@ -16,6 +16,8 @@

#include <RBDyn/FD.h>

#include <mc_rbdyn/ExternalTorqueSensor.h>

namespace mc_tvm
{

Expand Down Expand Up @@ -47,8 +49,8 @@ namespace mc_tvm
*/
struct MC_TVM_DLLAPI Robot : public tvm::graph::abstract::Node<Robot>
{
SET_OUTPUTS(Robot, FK, FV, FA, NormalAcceleration, tau, H, C)
SET_UPDATES(Robot, FK, FV, FA, NormalAcceleration, H, C)
SET_OUTPUTS(Robot, FK, FV, FA, NormalAcceleration, tau, H, C, ExternalForces)
SET_UPDATES(Robot, FK, FV, FA, NormalAcceleration, H, C, ExternalForces)

friend struct mc_rbdyn::Robot;
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
Expand Down Expand Up @@ -95,6 +97,10 @@ struct MC_TVM_DLLAPI Robot : public tvm::graph::abstract::Node<Robot>
inline const tvm::VariablePtr & alphaD() const noexcept { return ddq_; }
/** Access q second derivative (joint acceleration) */
inline tvm::VariablePtr & alphaD() noexcept { return ddq_; }
/** Access joint acceleration from external forces (const) */
inline const Eigen::VectorXd & alphaDExternal() const noexcept { return ddq_ext_; }
/** Access joint acceleration from external forces */
inline Eigen::VectorXd & alphaDExternal() noexcept { return ddq_ext_; }

/** Access floating-base variable (const) */
inline const tvm::VariablePtr & qFloatingBase() const noexcept { return q_fb_; }
Expand Down Expand Up @@ -133,6 +139,10 @@ struct MC_TVM_DLLAPI Robot : public tvm::graph::abstract::Node<Robot>
inline const tvm::VariablePtr & tau() const noexcept { return tau_; }
/** Access tau variable */
inline tvm::VariablePtr & tau() { return tau_; }
/** Access tau external variable (const) */
inline const Eigen::VectorXd & tauExternal() const noexcept { return tau_ext_; }
/** Access tau external variable */
inline Eigen::VectorXd & tauExternal() { return tau_ext_; }

/** Returns the CoM algorithm associated to this robot (const) */
inline const CoM & comAlgo() const noexcept { return *com_; }
Expand Down Expand Up @@ -203,8 +213,12 @@ struct MC_TVM_DLLAPI Robot : public tvm::graph::abstract::Node<Robot>
tvm::VariablePtr dq_;
/** Double derivative of q */
tvm::VariablePtr ddq_;
/** Joint acceleration from external forces */
Eigen::VectorXd ddq_ext_;
/** Tau variable */
tvm::VariablePtr tau_;
/** Tau external variable */
Eigen::VectorXd tau_ext_;
/** Normal accelerations of the bodies */
std::vector<sva::MotionVecd> normalAccB_;
/** Forward dynamics algorithm associated to this robot */
Expand All @@ -225,6 +239,7 @@ struct MC_TVM_DLLAPI Robot : public tvm::graph::abstract::Node<Robot>
void updateNormalAcceleration();
void updateH();
void updateC();
void updateEF();
};

} // namespace mc_tvm
8 changes: 8 additions & 0 deletions src/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -237,6 +237,8 @@ set(mc_rbdyn_SRC
mc_rbdyn/BodySensor.cpp
mc_rbdyn/Frame.cpp
mc_rbdyn/RobotFrame.cpp
mc_rbdyn/ExternalTorqueSensor.cpp
mc_rbdyn/VirtualTorqueSensor.cpp
)

set(mc_rbdyn_HDR
Expand Down Expand Up @@ -278,6 +280,8 @@ set(mc_rbdyn_HDR
../include/mc_rbdyn/Frame.h
../include/mc_rbdyn/RobotFrame.h
../include/mc_rbdyn/JointSensor.h
../include/mc_rbdyn/ExternalTorqueSensor.h
../include/mc_rbdyn/VirtualTorqueSensor.h
)

set(mc_tvm_HDR_DIR ../include/mc_tvm)
Expand Down Expand Up @@ -516,6 +520,8 @@ set(mc_tasks_SRC
mc_tasks/lipm_stabilizer/StabilizerTask_log_gui.cpp
mc_tasks/lipm_stabilizer/ZMPCC.cpp
mc_tasks/lipm_stabilizer/Contact.cpp
mc_tasks/CompliantEndEffectorTask.cpp
mc_tasks/CompliantPostureTask.cpp
)

set(mc_tasks_HDR
Expand Down Expand Up @@ -552,6 +558,8 @@ set(mc_tasks_HDR
../include/mc_tasks/lipm_stabilizer/StabilizerTask.h
../include/mc_tasks/lipm_stabilizer/Contact.h
../include/mc_tasks/lipm_stabilizer/ZMPCC.h
../include/mc_tasks/CompliantEndEffectorTask.h
../include/mc_tasks/CompliantPostureTask.h
)

add_library(mc_tasks SHARED ${mc_tasks_SRC} ${mc_tasks_HDR})
Expand Down
Loading