16 #include <RBDyn/MultiBody.h>
17 #include <RBDyn/MultiBodyConfig.h>
18 #include <RBDyn/MultiBodyGraph.h>
22 #include <unordered_map>
38 std::optional<sva::PTransformd> base_tf_ = std::nullopt;
40 std::optional<std::string> base_ = std::nullopt;
42 bool warn_on_missing_files_ =
false;
47 #define MAKE_LOAD_ROBOT_PARAMETER_SETTER(T, NAME) \
48 inline LoadRobotParameters & NAME(T value) noexcept \
59 #undef MAKE_LOAD_ROBOT_PARAMETER_SETTER
64 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
81 const std::string &
name()
const;
109 return data_->bodySensorsIndex.count(name) != 0;
119 return data_->bodyBodySensors.count(body) != 0;
153 return data()->jointJointSensors.count(joint) != 0;
166 inline const std::vector<JointSensor> &
jointSensors() const noexcept {
return data_->jointSensors; }
178 inline bool hasFrame(
const std::string & name)
const noexcept {
return frames_.count(name) != 0; }
186 auto it = frames_.find(name);
187 if(it != frames_.end()) {
return *it->second; }
197 return const_cast<RobotFrame &
>(
static_cast<const Robot *
>(
this)->frame(name));
239 sva::PTransformd X_p_f,
240 bool baked =
false)
const;
249 void makeFrames(std::vector<mc_rbdyn::RobotModule::FrameDescription> frames);
278 rbd::MultiBody &
mb();
280 const rbd::MultiBody &
mb()
const;
283 rbd::MultiBodyConfig &
mbc();
285 const rbd::MultiBodyConfig &
mbc()
const;
288 rbd::MultiBodyGraph &
mbg();
290 const rbd::MultiBodyGraph &
mbg()
const;
293 const std::vector<std::vector<double>> &
q()
const;
295 const std::vector<std::vector<double>> &
alpha()
const;
297 const std::vector<std::vector<double>> &
alphaD()
const;
301 inline const std::vector<std::vector<double>> &
controlTorque() const noexcept {
return jointTorque(); }
303 const std::vector<sva::PTransformd> &
bodyPosW()
const;
305 const std::vector<sva::MotionVecd> &
bodyVelW()
const;
307 const std::vector<sva::MotionVecd> &
bodyVelB()
const;
309 const std::vector<sva::MotionVecd> &
bodyAccB()
const;
311 std::vector<std::vector<double>> &
q();
313 std::vector<std::vector<double>> &
alpha();
315 std::vector<std::vector<double>> &
alphaD();
319 inline std::vector<std::vector<double>> &
controlTorque() noexcept {
return jointTorque(); }
333 const sva::PTransformd &
bodyPosW(
const std::string & name)
const;
341 sva::PTransformd
X_b1_b2(
const std::string & b1,
const std::string & b2)
const;
347 const sva::MotionVecd &
bodyVelW(
const std::string & name)
const;
353 const sva::MotionVecd &
bodyVelB(
const std::string & name)
const;
359 const sva::MotionVecd &
bodyAccB(
const std::string & name)
const;
362 Eigen::Vector3d
com()
const;
396 sva::ForceVecd
bodyWrench(
const std::string & bodyName)
const;
409 Eigen::Vector2d
cop(
const std::string & frame,
double min_pressure = 0.5)
const;
422 Eigen::Vector3d
copW(
const std::string & frame,
double min_pressure = 0.5)
const;
431 sva::ForceVecd
netWrench(
const std::vector<std::string> & sensorNames)
const;
453 Eigen::Vector3d
zmp(
const sva::ForceVecd & netTotalWrench,
454 const Eigen::Vector3d & plane_p,
455 const Eigen::Vector3d & plane_n,
456 double minimalNetNormalForce = 1.)
const;
473 bool zmp(Eigen::Vector3d & zmpOut,
474 const sva::ForceVecd & netTotalWrench,
475 const Eigen::Vector3d & plane_p,
476 const Eigen::Vector3d & plane_n,
477 double minimalNetNormalForce = 1.) const noexcept;
494 Eigen::Vector3d
zmp(const sva::ForceVecd & netTotalWrench,
495 const sva::PTransformd & zmpFrame,
496 double minimalNetNormalForce = 1.) const;
514 bool zmp(Eigen::Vector3d & zmpOut,
515 const sva::ForceVecd & netTotalWrench,
516 const sva::PTransformd & zmpFrame,
517 double minimalNetNormalForce = 1.) const noexcept;
525 Eigen::Vector3d
zmp(const
std::vector<
std::
string> & sensorNames,
526 const Eigen::Vector3d & plane_p,
527 const Eigen::Vector3d & plane_n,
528 double minimalNetNormalForce = 1.) const;
536 bool zmp(Eigen::Vector3d & zmpOut,
537 const
std::vector<
std::
string> & sensorNames,
538 const Eigen::Vector3d & plane_p,
539 const Eigen::Vector3d & plane_n,
540 double minimalNetNormalForce = 1.) const noexcept;
549 Eigen::Vector3d
zmp(const
std::vector<
std::
string> & sensorNames,
550 const sva::PTransformd & zmpFrame,
551 double minimalNetNormalForce = 1.) const;
560 bool zmp(Eigen::Vector3d & zmpOut,
561 const
std::vector<
std::
string> & sensorNames,
562 const sva::PTransformd & zmpFrame,
563 double minimalNetNormalForce = 1.) const noexcept;
566 const
std::vector<
std::vector<
double>> & ql() const;
568 const
std::vector<
std::vector<
double>> & qu() const;
570 const
std::vector<
std::vector<
double>> & vl() const;
572 const
std::vector<
std::vector<
double>> & vu() const;
574 const
std::vector<
std::vector<
double>> & al() const;
576 const
std::vector<
std::vector<
double>> & au() const;
578 const
std::vector<
std::vector<
double>> & jl() const;
580 const
std::vector<
std::vector<
double>> & ju() const;
582 const
std::vector<
std::vector<
double>> & tl() const;
584 const
std::vector<
std::vector<
double>> & tu() const;
586 const
std::vector<
std::vector<
double>> & tdl() const;
588 const
std::vector<
std::vector<
double>> & tdu() const;
610 std::vector<
std::vector<
double>> & tdl();
612 std::vector<
std::vector<
double>> & tdu();
626 void zmpTarget(const Eigen::Vector3d &
zmp);
632 const Eigen::Vector3d & zmpTarget() const;
635 inline
double mass() const noexcept {
return mass_; }
645 inline const std::vector<double> &
encoderValues() const noexcept {
return data_->encoderValues; }
648 inline const std::vector<double> &
encoderVelocities() const noexcept {
return data_->encoderVelocities; }
651 inline const std::vector<double> &
jointTorques() const noexcept {
return data_->jointTorques; }
654 inline const std::vector<std::string> &
refJointOrder()
const {
return data_->refJointOrder; }
687 return data_->forceSensorsIndex.count(name) != 0;
702 return data_->bodyForceSensors_.count(body) != 0;
720 return bodyHasForceSensor(this->surface(surface).bodyName());
734 return bodyHasForceSensor(body) || findIndirectForceSensorBodyName(body).empty();
747 return bodyHasIndirectForceSensor(this->surface(surface).bodyName());
820 inline const std::vector<ForceSensor> &
forceSensors() const noexcept {
return data_->forceSensors; }
840 bool hasDevice(
const std::string & name)
const noexcept;
844 inline bool hasSensor(
const std::string & name)
const noexcept
846 return hasDevice<T>(name);
861 const T &
device(
const std::string & name)
const;
867 return const_cast<T &
>(
const_cast<const Robot *
>(
this)->device<T>(name));
875 inline const T &
sensor(
const std::string & name)
const
877 return device<T>(name);
882 inline T &
sensor(
const std::string & name)
884 return device<T>(name);
921 const std::map<std::string, mc_rbdyn::SurfacePtr> &
surfaces()
const;
945 const std::map<std::string, convex_pair_t> &
convexes()
const;
961 const std::string & body,
963 const sva::PTransformd & X_b_c = sva::PTransformd::Identity());
998 std::map<std::string, std::vector<double>>
stance()
const;
1023 const sva::MotionVecd & A_0 = sva::MotionVecd(Eigen::Vector6d::Zero()))
const;
1031 const sva::PTransformd &
posW()
const;
1043 void posW(
const sva::PTransformd & pt);
1051 void velW(
const sva::MotionVecd & vel);
1054 const sva::MotionVecd &
velW()
const;
1062 void accW(
const sva::MotionVecd & acc);
1067 const sva::MotionVecd
accW()
const;
1088 inline const std::unordered_map<std::string, mc_control::GripperPtr> &
grippersByName() const noexcept
1090 return data_->grippers;
1094 inline const std::vector<mc_control::GripperRef> &
grippers() const noexcept {
return data_->grippersRef; }
1119 unsigned int robots_idx_;
1121 Eigen::Vector3d zmp_;
1122 std::vector<sva::PTransformd> bodyTransforms_;
1123 std::vector<std::vector<double>> ql_;
1124 std::vector<std::vector<double>> qu_;
1125 std::vector<std::vector<double>> vl_;
1126 std::vector<std::vector<double>> vu_;
1127 std::vector<std::vector<double>> al_;
1128 std::vector<std::vector<double>> au_;
1129 std::vector<std::vector<double>> jl_;
1130 std::vector<std::vector<double>> ju_;
1131 std::vector<std::vector<double>> tl_;
1132 std::vector<std::vector<double>> tu_;
1133 std::vector<std::vector<double>> tdl_;
1134 std::vector<std::vector<double>> tdu_;
1135 std::map<std::string, convex_pair_t> convexes_;
1136 std::map<std::string, sva::PTransformd> collisionTransforms_;
1137 std::map<std::string, mc_rbdyn::SurfacePtr> surfaces_;
1138 std::map<std::string, std::vector<double>> stance_;
1143 std::vector<int> refJointIndexToMBCIndex_;
1147 std::vector<Flexibility> flexibility_;
1149 std::unordered_map<std::string, RobotFramePtr> frames_;
1166 const std::string & name,
1168 unsigned int robots_idx,
1214 mutable std::map<std::string, mc_tvm::ConvexPtr> tvm_convexes_;
1221 void name(
const std::string & n);
1262 const std::string & prefix,
1263 bool required =
false,
1264 const std::string & robotIndexKey =
"robotIndex",
1265 const std::string & robotNameKey =
"robot",
1266 const std::string & defaultRobotName =
"");
1276 const std::string & prefix =
"",
1277 bool required =
false,
1278 const std::string & robotIndexKey =
"robotIndex",
1279 const std::string & robotNameKey =
"robot",
1280 const std::string & defaultRobotName =
"");
1294 const std::string & prefix =
"",
1295 bool required =
false,
1296 const std::string & robotIndexKey =
"robotIndex",
1297 const std::string & robotNameKey =
"robot",
1298 const std::string & defaultRobotName =
"");
1308 #include <mc_rbdyn/Robot.hpp>
std::string MC_RBDYN_DLLAPI robotNameFromConfig(const mc_rtc::Configuration &config, const mc_rbdyn::Robots &robots, const std::string &prefix="", bool required=false, const std::string &robotIndexKey="robotIndex", const std::string &robotNameKey="robot", const std::string &defaultRobotName="")
Helper to obtain the robot name from configuration.
MC_RBDYN_DLLAPI const mc_rbdyn::Robot & robotFromConfig(const mc_rtc::Configuration &config, const mc_rbdyn::Robots &robots, const std::string &prefix, bool required=false, const std::string &robotIndexKey="robotIndex", const std::string &robotNameKey="robot", const std::string &defaultRobotName="")
Obtains a reference to a loaded robot from configuration (using robotName or robotIndex)
unsigned int MC_RBDYN_DLLAPI robotIndexFromConfig(const mc_rtc::Configuration &config, const mc_rbdyn::Robots &robots, const std::string &prefix="", bool required=false, const std::string &robotIndexKey="robotIndex", const std::string &robotNameKey="robot", const std::string &defaultRobotName="")
Helper to obtain the robot index from configuration.
#define MC_RBDYN_DLLAPI
Definition: api.h:50
#define MAKE_LOAD_ROBOT_PARAMETER_SETTER(T, NAME)
Definition: Robot.h:47
Definition: generic_gripper.h:15
Eigen::Vector3d MC_RBDYN_DLLAPI zmp(const sva::ForceVecd &netTotalWrench, const Eigen::Vector3d &plane_p, const Eigen::Vector3d &plane_n, double minimalNetNormalForce=1.)
Actual ZMP computation from net total wrench and the ZMP plane.
std::shared_ptr< RobotData > RobotDataPtr
Definition: RobotData.h:73
std::unique_ptr< Device > DevicePtr
Definition: Device.h:20
std::shared_ptr< RobotFrame > RobotFramePtr
Definition: fwd.h:24
std::vector< BodySensor, Eigen::aligned_allocator< BodySensor > > BodySensorVector
Definition: BodySensor.h:145
std::shared_ptr< Surface > SurfacePtr
Definition: Surface.h:72
DevicePtr SensorPtr
Definition: Device.h:22
std::shared_ptr< sch::S_Object > S_ObjectPtr
Definition: RobotModule.h:35
auto Robot(const std::string &name, GetT get_fn)
Definition: Robot.h:56
void error_and_throw(Args &&... args)
Definition: logging.h:47
std::unique_ptr< Robot > RobotPtr
Definition: fwd.h:25
A robot's gripper reprensentation.
Definition: generic_gripper.h:35
Definition: BodySensor.h:20
Definition: RobotModule.h:42
Definition: Flexibility.h:25
Definition: ForceSensor.h:20
Definition: JointSensor.h:18
Definition: RobotFrame.h:22
Definition: RobotModule.h:70
rbd::MultiBodyConfig & mbc()
void fixCollisionTransforms()
const std::vector< std::string > & refJointOrder() const
Definition: Robot.h:654
const sva::PTransformd & bodyTransform(int bodyIndex) const
void addSurface(mc_rbdyn::SurfacePtr surface, bool doNotReplace=true)
const mc_rbdyn::Surface & surface(const std::string &sName) const
const std::vector< JointSensor > & jointSensors() const noexcept
Definition: Robot.h:166
sva::PTransformd surfacePose(const std::string &sName) const
const ForceSensor & indirectBodyForceSensor(const std::string &body) const
Return a force sensor directly or indirectly attached to a body.
sva::ForceVecd netWrench(const std::vector< std::string > &sensorNames) const
Computes net total wrench from a list of sensors.
const std::vector< sva::PTransformd > & bodyTransforms() const
void loadRSDFFromDir(const std::string &surfaceDir)
void addDevice(DevicePtr device)
const std::vector< double > & encoderValues() const noexcept
Definition: Robot.h:645
const std::vector< sva::MotionVecd > & bodyAccB() const
mc_tvm::Convex & tvmConvex(const std::string &name) const
sva::ForceVecd bodyWrench(const std::string &bodyName) const
std::vector< sva::MotionVecd > & bodyVelW()
void addForceSensor(const mc_rbdyn::ForceSensor &fs)
const convex_pair_t & convex(const std::string &cName) const
Eigen::Vector3d copW(const std::string &frame, double min_pressure=0.5) const
const sva::MotionVecd accW() const
void removeConvex(const std::string &name)
int jointIndexInMBC(size_t jointIndex) const
const sva::MotionVecd & velW() const
Eigen::Vector3d zmp(const sva::ForceVecd &netTotalWrench, const Eigen::Vector3d &plane_p, const Eigen::Vector3d &plane_n, double minimalNetNormalForce=1.) const
Actual ZMP computation from net total wrench and the ZMP plane.
void fixSurface(Surface &surface)
const RobotModule & module() const
const ForceSensor & forceSensor(const std::string &name) const
Robot & operator=(Robot &&)
const BodySensor & bodySensor() const noexcept
Definition: Robot.h:94
bool hasForceSensor(const std::string &name) const noexcept
Definition: Robot.h:685
const std::vector< mc_control::GripperRef > & grippers() const noexcept
Definition: Robot.h:1094
bool hasSurface(const std::string &surface) const
const std::vector< std::vector< double > > & q() const
const sva::PTransformd & posW() const
mc_rbdyn::Robots & robots() noexcept
Definition: Robot.h:1001
bool hasFrame(const std::string &name) const noexcept
Definition: Robot.h:178
const T & device(const std::string &name) const
Eigen::Vector3d com() const
void velW(const sva::MotionVecd &vel)
bool hasBodySensor(const std::string &name) const noexcept
Definition: Robot.h:107
unsigned int bodyIndexByName(const std::string &name) const
std::vector< sva::MotionVecd > & bodyVelB()
void posW(const sva::PTransformd &pt)
const std::vector< std::vector< double > > & alpha() const
bool hasSensor(const std::string &name) const noexcept
Definition: Robot.h:844
bool hasDevice(const std::string &name) const noexcept
std::vector< std::vector< double > > & alphaD()
const JointSensor & jointJointSensor(const std::string &joint) const
Eigen::Vector3d comAcceleration() const
const mc_rbdyn::Robots & robots() const noexcept
Definition: Robot.h:1004
T & device(const std::string &name)
Definition: Robot.h:865
const BodySensor & bodySensor(const std::string &name) const
T & sensor(const std::string &name)
Definition: Robot.h:882
void forwardVelocity(rbd::MultiBodyConfig &mbc) const
bool hasConvex(const std::string &name) const
sva::PTransformd X_b1_b2(const std::string &b1, const std::string &b2) const
const ForceSensor & indirectSurfaceForceSensor(const std::string &surface) const
Return a force sensor directly or indirectly attached to a surface.
const rbd::MultiBodyGraph & mbg() const
unsigned int jointIndexByName(const std::string &name) const
const ForceSensor & bodyForceSensor(const std::string &body) const
const std::vector< sva::PTransformd > & bodyPosW() const
convex_pair_t & convex(const std::string &cName)
bool surfaceHasForceSensor(const std::string &surface) const
Checks if the surface has a force sensor directly attached to it.
Definition: Robot.h:718
const sva::MotionVecd & bodyAccB(const std::string &name) const
bool hasBody(const std::string &name) const
const std::vector< std::vector< double > > & controlTorque() const noexcept
Definition: Robot.h:301
const sva::MotionVecd & bodyVelB(const std::string &name) const
const std::vector< double > & encoderVelocities() const noexcept
Definition: Robot.h:648
std::vector< std::vector< double > > & q()
mc_control::Gripper & gripper(const std::string &gripper)
std::vector< std::string > frames() const
void accW(const sva::MotionVecd &acc)
bool jointHasJointSensor(const std::string &joint) const noexcept
Definition: Robot.h:151
rbd::MultiBodyGraph & mbg()
const sva::PTransformd & bodyTransform(const std::string &bName) const
sva::ForceVecd surfaceWrench(const std::string &surfaceName) const
void forwardKinematics(rbd::MultiBodyConfig &mbc) const
const std::vector< std::vector< double > > & jointTorque() const
mc_rbdyn::Surface & copySurface(const std::string &sName, const std::string &name)
const std::string & name() const
const std::map< std::string, convex_pair_t > & convexes() const
Eigen::Vector3d comVelocity() const
Eigen::Vector2d cop(const std::string &frame, double min_pressure=0.5) const
mc_rbdyn::Surface & surface(const std::string &sName)
RobotFramePtr makeTemporaryFrame(const std::string &name, const RobotFrame &parent, sva::PTransformd X_p_f, bool baked=false) const
std::vector< std::string > availableSurfaces() const
const std::vector< sva::MotionVecd > & bodyVelW() const
bool bodyHasBodySensor(const std::string &body) const noexcept
Definition: Robot.h:117
bool hasJoint(const std::string &name) const
const rbd::MultiBody & mb() const
void makeFrames(std::vector< mc_rbdyn::RobotModule::FrameDescription > frames)
void forwardAcceleration(const sva::MotionVecd &A_0=sva::MotionVecd(Eigen::Vector6d::Zero()))
void addSensor(SensorPtr sensor)
Definition: Robot.h:891
const sva::MotionVecd & bodyVelW(const std::string &name) const
const mc_control::Gripper & gripper(const std::string &gripper) const
const std::vector< ForceSensor > & forceSensors() const noexcept
Definition: Robot.h:820
std::map< std::string, std::vector< double > > stance() const
const std::vector< sva::MotionVecd > & bodyVelB() const
const BodySensor & bodyBodySensor(const std::string &name) const
const DevicePtrVector & devices() const noexcept
Definition: Robot.h:871
const RobotFrame & frame(const std::string &name) const
Definition: Robot.h:184
std::vector< std::vector< double > > & controlTorque() noexcept
Definition: Robot.h:319
const T & sensor(const std::string &name) const
Definition: Robot.h:875
std::vector< sva::MotionVecd > & bodyAccB()
void eulerIntegration(double step)
const ForceSensor & surfaceForceSensor(const std::string &surfaceName) const
bool bodyHasForceSensor(const std::string &body) const noexcept
Definition: Robot.h:700
bool zmp(Eigen::Vector3d &zmpOut, const sva::ForceVecd &netTotalWrench, const Eigen::Vector3d &plane_p, const Eigen::Vector3d &plane_n, double minimalNetNormalForce=1.) const noexcept
Actual ZMP computation from net total wrench and the ZMP plane.
std::vector< std::vector< double > > & alpha()
RobotFrame & frame(const std::string &name)
Definition: Robot.h:195
RobotFrame & makeFrame(const std::string &name, RobotFrame &parent, sva::PTransformd X_p_f, bool baked=false)
void forwardAcceleration(rbd::MultiBodyConfig &mbc, const sva::MotionVecd &A_0=sva::MotionVecd(Eigen::Vector6d::Zero())) const
std::string findIndirectForceSensorBodyName(const std::string &bodyName) const
Finds the name of the body to which a force sensor is attached, starting from the provided body and g...
std::vector< sva::PTransformd > & bodyPosW()
bool surfaceHasIndirectForceSensor(const std::string &surface) const
Definition: Robot.h:745
bool hasGripper(const std::string &gripper) const
void copyLoadedData(Robot &destination) const
void addConvex(const std::string &name, const std::string &body, S_ObjectPtr convex, const sva::PTransformd &X_b_c=sva::PTransformd::Identity())
const std::unordered_map< std::string, mc_control::GripperPtr > & grippersByName() const noexcept
Definition: Robot.h:1088
mc_tvm::Robot & tvmRobot() const
void eulerIntegration(rbd::MultiBodyConfig &mbc, double step) const
void addBodySensor(const BodySensor &sensor)
std::pair< std::string, S_ObjectPtr > convex_pair_t
Definition: Robot.h:68
const std::vector< std::vector< double > > & alphaD() const
const rbd::MultiBodyConfig & mbc() const
const std::map< std::string, mc_rbdyn::SurfacePtr > & surfaces() const
bool bodyHasIndirectForceSensor(const std::string &body) const
Definition: Robot.h:732
unsigned int robotIndex() const
const sva::PTransformd & collisionTransform(const std::string &cName) const
const RobotDataPtr data() const noexcept
Definition: Robot.h:1097
Robot(NewRobotToken, const std::string &name, Robots &robots, unsigned int robots_idx, bool loadFiles, const LoadRobotParameters ¶ms={})
const std::vector< double > & jointTorques() const noexcept
Definition: Robot.h:651
const sva::PTransformd & bodyPosW(const std::string &name) const
std::vector< std::vector< double > > & jointTorque()
const BodySensorVector & bodySensors() const noexcept
Definition: Robot.h:141
const ForceSensor * findBodyForceSensor(const std::string &body) const
Simplify access to values hold within a JSON file.
Definition: Configuration.h:166