Biorbd Release_1.13.0
Loading...
Searching...
No Matches
SoftContacts.cpp
1#define BIORBD_API_EXPORTS
2#include "RigidBody/SoftContacts.h"
3
4#include "RigidBody/GeneralizedCoordinates.h"
5#include "RigidBody/GeneralizedVelocity.h"
6#include "RigidBody/Joints.h"
7#include "RigidBody/Segment.h"
8#include "RigidBody/SoftContactNode.h"
9#include "RigidBody/SoftContactSphere.h"
10#include "Utils/Error.h"
11#include "Utils/SpatialVector.h"
12#include "Utils/String.h"
13#include "Utils/UtilsEnum.h"
14
15using namespace BIORBD_NAMESPACE;
16
19 std::make_shared<std::vector<std::shared_ptr<SoftContactNode>>>()) {}
20
26
28 m_softContacts->resize(other.m_softContacts->size());
29 for (size_t i = 0; i < other.m_softContacts->size(); ++i) {
30 if ((*other.m_softContacts)[i]->typeOfNode() ==
31 utils::NODE_TYPE::SOFT_CONTACT_SPHERE) {
32 (*m_softContacts)[i] = std::make_shared<rigidbody::SoftContactSphere>();
33 } else {
34 utils::Error::raise("DeepCopy failed");
35 }
36 (*m_softContacts)[i]->DeepCopy(*((*other.m_softContacts)[i]));
37 }
38}
39
41 return (*m_softContacts)[i]->utils::Node::name();
42}
43
44std::vector<utils::String> rigidbody::SoftContacts::softContactNames() {
45 std::vector<utils::String> out;
46 for (auto &c : *m_softContacts) {
47 out.push_back(c->utils::Node::name());
48 }
49 return out;
50}
51
53 const rigidbody::SoftContactNode &contact) {
54 if (contact.typeOfNode() == utils::NODE_TYPE::SOFT_CONTACT_SPHERE) {
55 m_softContacts->push_back(
56 std::make_shared<rigidbody::SoftContactSphere>(contact));
57 } else {
59 utils::String("The ") + contact.typeOfNode() + " does not exist");
60 }
61}
62
64 return *(*m_softContacts)[idx];
65}
66
69 size_t idx,
70 bool updateKin) {
71 rigidbody::Joints &model = dynamic_cast<rigidbody::Joints &>(*this);
72
75 model.CalcBodyToBaseCoordinates(Q, sc.parent(), sc, updateKin));
76}
77
78std::vector<rigidbody::NodeSegment> rigidbody::SoftContacts::softContacts(
80 bool updateKin) {
81 std::vector<rigidbody::NodeSegment> out;
82 for (int i = 0; i < static_cast<int>(m_softContacts->size()); ++i) {
83 out.push_back(softContact(Q, i, updateKin));
84#ifndef BIORBD_USE_CASADI_MATH
85 updateKin = false;
86#endif
87 }
88 return out;
89}
90
94 size_t idx,
95 bool updateKin) {
96 // Assuming that this is also a joint type (via BiorbdModel)
97 rigidbody::Joints &model = dynamic_cast<rigidbody::Joints &>(*this);
98
99 // Calculate the velocity of the point
102 model.CalcPointVelocity(Q, Qdot, sc.parent(), sc, updateKin));
103}
104
108 size_t idx,
109 bool updateKin) {
110 // Assuming that this is also a joint type (via BiorbdModel)
111 rigidbody::Joints &model = dynamic_cast<rigidbody::Joints &>(*this);
112
113 // Calculate the velocity of the point
116 model.CalcPointVelocity6D(Q, Qdot, sc.parent(), sc, updateKin)
117 .block(0, 0, 3, 1));
118}
119
120std::vector<rigidbody::NodeSegment>
124 bool updateKin) {
125 std::vector<rigidbody::NodeSegment> pos;
126 for (size_t i = 0; i < nbSoftContacts(); ++i) {
127 pos.push_back(softContactVelocity(Q, Qdot, i, updateKin));
128 updateKin = false;
129 }
130
131 return pos;
132}
133
134std::vector<rigidbody::NodeSegment>
138 bool updateKin) {
139 std::vector<rigidbody::NodeSegment> pos;
140 for (size_t i = 0; i < nbSoftContacts(); ++i) {
141 pos.push_back(softContactAngularVelocity(Q, Qdot, i, updateKin));
142 updateKin = false;
143 }
144
145 return pos;
146}
147
149 return m_softContacts->size();
150}
151
153 size_t idx) const {
154 // Assuming that this is also a joint type (via BiorbdModel)
155 const rigidbody::Joints &model =
156 dynamic_cast<const rigidbody::Joints &>(*this);
157
158 // Name of the segment to find
159 const utils::String &name(model.segment(idx).name());
160
161 std::vector<size_t> indices;
162 for (size_t i = 0; i < nbSoftContacts();
163 ++i) // Go through all the markers and select the right ones
164 if (!(*m_softContacts)[i]->parent().compare(name)) {
165 indices.push_back(i);
166 }
167
168 return indices;
169}
This is the core of the musculoskeletal model in biorbd.
Definition Joints.h:48
utils::SpatialVector CalcPointVelocity6D(const rigidbody::GeneralizedCoordinates &Q, const rigidbody::GeneralizedVelocity &Qdot, utils::String segmentName, const utils::Vector3d &pointInLocal, bool updateKin=true)
Interface to call CalcPointVelocity6D. It is pretty much useless in Eigen, but fixes an issue in Casa...
Definition Joints.cpp:443
Segment & segment(size_t idx)
Get a segment of index idx.
Definition Joints.cpp:205
utils::Vector3d CalcPointVelocity(const rigidbody::GeneralizedCoordinates &Q, const rigidbody::GeneralizedVelocity &Qdot, utils::String segmentName, const utils::Vector3d &pointInLocal, bool updateKin=true)
Interface to call CalcPointVelocity. It is pretty much useless in Eigen, but fixes an issue in Casadi...
Definition Joints.cpp:415
utils::Vector3d CalcBodyToBaseCoordinates(const rigidbody::GeneralizedCoordinates &Q, utils::String segmentName, const utils::Vector3d &pointInLocal, bool updateKin=true)
Interface to call CalcBodyToBaseCoordinates. It is pretty much useless in Eigen, but fixes an issue i...
Definition Joints.cpp:390
A point attached to a segment, generally speaking a skin marker.
Definition NodeSegment.h:20
A point attached to a contact node, generally speaking a skin marker.
Holder for the biorbd contact set.
NodeSegment softContactVelocity(const GeneralizedCoordinates &Q, const GeneralizedVelocity &Qdot, size_t idx, bool updateKin=true)
Return the linear velocity of a contact.
std::shared_ptr< std::vector< std::shared_ptr< SoftContactNode > > > m_softContacts
The contacts.
std::vector< NodeSegment > softContactsVelocity(const GeneralizedCoordinates &Q, const GeneralizedVelocity &Qdot, bool updateKin=true)
Return the velocity of all the contacts.
std::vector< NodeSegment > softContacts(const GeneralizedCoordinates &Q, bool updateKin=true)
Return a the contacts at a given position Q.
utils::String softContactName(size_t i)
Return the name of the soft contact.
SoftContactNode & softContact(size_t idx)
Return a specified contact.
std::vector< size_t > segmentSoftContactIdx(size_t idx) const
Return all the soft contacts indices of a segment.
void addSoftContact(const SoftContactNode &contact)
Add a new contact to the contact set.
NodeSegment softContactAngularVelocity(const GeneralizedCoordinates &Q, const GeneralizedVelocity &Qdot, size_t idx, bool updateKin=true)
Return the angular velocity of a contact.
std::vector< utils::String > softContactNames()
Return the names of the soft contact.
std::vector< NodeSegment > softContactsAngularVelocity(const GeneralizedCoordinates &Q, const GeneralizedVelocity &Qdot, bool updateKin=true)
Return the angular velocity of all the contacts.
SoftContacts()
Construct a contact biorbd set.
SoftContacts DeepCopy() const
Deep copy of the contacts.
size_t nbSoftContacts() const
Return the number of contacts.
static void raise(const String &message)
Throw an error message.
Definition Error.cpp:6
const String & name() const
Return the name of the node.
Definition Node.cpp:43
NODE_TYPE typeOfNode() const
Return the type of node.
Definition Node.cpp:49
const String & parent() const
Return the parent name of the node.
Definition Node.cpp:45
Wrapper around the std::string class with augmented functionality.
Definition String.h:18