11 template <
typename Scalar>
12 ContactModelMultipleTpl<Scalar>::ContactModelMultipleTpl(boost::shared_ptr<StateMultibody> state,
13 const std::size_t& nu)
14 : state_(state), nc_(0), nc_total_(0), nu_(nu) {}
16 template <
typename Scalar>
17 ContactModelMultipleTpl<Scalar>::ContactModelMultipleTpl(boost::shared_ptr<StateMultibody> state)
18 : state_(state), nc_(0), nc_total_(0), nu_(state->get_nv()) {}
20 template <
typename Scalar>
21 ContactModelMultipleTpl<Scalar>::~ContactModelMultipleTpl() {}
23 template <
typename Scalar>
24 void ContactModelMultipleTpl<Scalar>::addContact(
const std::string& name,
25 boost::shared_ptr<ContactModelAbstract> contact,
bool active) {
26 if (contact->get_nu() != nu_) {
27 throw_pretty(
"Invalid argument: " 28 <<
"contact item doesn't have the same control dimension (" + std::to_string(nu_) +
")");
30 std::pair<typename ContactModelContainer::iterator, bool> ret =
31 contacts_.insert(std::make_pair(name, boost::make_shared<ContactItem>(name, contact, active)));
32 if (ret.second ==
false) {
33 std::cout <<
"Warning: this contact item already existed, we cannot add it" << std::endl;
35 nc_ += contact->get_nc();
36 nc_total_ += contact->get_nc();
38 nc_total_ += contact->get_nc();
42 template <
typename Scalar>
43 void ContactModelMultipleTpl<Scalar>::removeContact(
const std::string& name) {
44 typename ContactModelContainer::iterator it = contacts_.find(name);
45 if (it != contacts_.end()) {
46 nc_ -= it->second->contact->get_nc();
47 nc_total_ -= it->second->contact->get_nc();
50 std::cout <<
"Warning: this contact item doesn't exist, we cannot remove it" << std::endl;
54 template <
typename Scalar>
55 void ContactModelMultipleTpl<Scalar>::changeContactStatus(
const std::string& name,
bool active) {
56 typename ContactModelContainer::iterator it = contacts_.find(name);
57 if (it != contacts_.end()) {
58 if (active && !it->second->active) {
59 nc_ += it->second->contact->get_nc();
60 }
else if (!active && it->second->active) {
61 nc_ -= it->second->contact->get_nc();
63 it->second->active = active;
65 std::cout <<
"Warning: this contact item doesn't exist, we cannot change its status" << std::endl;
69 template <
typename Scalar>
70 void ContactModelMultipleTpl<Scalar>::calc(
const boost::shared_ptr<ContactDataMultiple>& data,
71 const Eigen::Ref<const VectorXs>& x) {
72 if (data->contacts.size() != contacts_.size()) {
73 throw_pretty(
"Invalid argument: " 74 <<
"it doesn't match the number of contact datas and models");
78 const std::size_t& nv = state_->get_nv();
79 typename ContactModelContainer::iterator it_m, end_m;
80 typename ContactDataContainer::iterator it_d, end_d;
81 for (it_m = contacts_.begin(), end_m = contacts_.end(), it_d = data->contacts.begin(), end_d = data->contacts.end();
82 it_m != end_m || it_d != end_d; ++it_m, ++it_d) {
83 const boost::shared_ptr<ContactItem>& m_i = it_m->second;
85 const boost::shared_ptr<ContactDataAbstract>& d_i = it_d->second;
86 assert_pretty(it_m->first == it_d->first,
"it doesn't match the contact name between data and model");
88 m_i->contact->calc(d_i, x);
89 const std::size_t& nc_i = m_i->contact->get_nc();
90 data->a0.segment(nc, nc_i) = d_i->a0;
91 data->Jc.block(nc, 0, nc_i, nv) = d_i->Jc;
97 template <
typename Scalar>
98 void ContactModelMultipleTpl<Scalar>::calcDiff(
const boost::shared_ptr<ContactDataMultiple>& data,
99 const Eigen::Ref<const VectorXs>& x) {
100 if (data->contacts.size() != contacts_.size()) {
101 throw_pretty(
"Invalid argument: " 102 <<
"it doesn't match the number of contact datas and models");
106 const std::size_t& ndx = state_->get_ndx();
107 typename ContactModelContainer::iterator it_m, end_m;
108 typename ContactDataContainer::iterator it_d, end_d;
109 for (it_m = contacts_.begin(), end_m = contacts_.end(), it_d = data->contacts.begin(), end_d = data->contacts.end();
110 it_m != end_m || it_d != end_d; ++it_m, ++it_d) {
111 const boost::shared_ptr<ContactItem>& m_i = it_m->second;
113 const boost::shared_ptr<ContactDataAbstract>& d_i = it_d->second;
114 assert_pretty(it_m->first == it_d->first,
"it doesn't match the contact name between data and model");
116 m_i->contact->calcDiff(d_i, x);
117 const std::size_t& nc_i = m_i->contact->get_nc();
118 data->da0_dx.block(nc, 0, nc_i, ndx) = d_i->da0_dx;
124 template <
typename Scalar>
125 void ContactModelMultipleTpl<Scalar>::updateAcceleration(
const boost::shared_ptr<ContactDataMultiple>& data,
126 const VectorXs& dv)
const {
127 if (static_cast<std::size_t>(dv.size()) != state_->get_nv()) {
128 throw_pretty(
"Invalid argument: " 129 <<
"dv has wrong dimension (it should be " + std::to_string(state_->get_nv()) +
")");
134 template <
typename Scalar>
135 void ContactModelMultipleTpl<Scalar>::updateForce(
const boost::shared_ptr<ContactDataMultiple>& data,
136 const VectorXs& force) {
137 if (static_cast<std::size_t>(force.size()) != nc_) {
138 throw_pretty(
"Invalid argument: " 139 <<
"force has wrong dimension (it should be " + std::to_string(nc_) +
")");
141 if (static_cast<std::size_t>(data->contacts.size()) != contacts_.size()) {
142 throw_pretty(
"Invalid argument: " 143 <<
"it doesn't match the number of contact datas and models");
147 for (ForceIterator it = data->fext.begin(); it != data->fext.end(); ++it) {
148 *it = pinocchio::ForceTpl<Scalar>::Zero();
151 typename ContactModelContainer::iterator it_m, end_m;
152 typename ContactDataContainer::iterator it_d, end_d;
153 for (it_m = contacts_.begin(), end_m = contacts_.end(), it_d = data->contacts.begin(), end_d = data->contacts.end();
154 it_m != end_m || it_d != end_d; ++it_m, ++it_d) {
155 const boost::shared_ptr<ContactItem>& m_i = it_m->second;
157 const boost::shared_ptr<ContactDataAbstract>& d_i = it_d->second;
158 assert_pretty(it_m->first == it_d->first,
"it doesn't match the contact name between data and model");
160 const std::size_t& nc_i = m_i->contact->get_nc();
161 const Eigen::VectorBlock<const VectorXs, Eigen::Dynamic> force_i = force.segment(nc, nc_i);
162 m_i->contact->updateForce(d_i, force_i);
163 data->fext[d_i->joint] = d_i->f;
169 template <
typename Scalar>
170 void ContactModelMultipleTpl<Scalar>::updateAccelerationDiff(
const boost::shared_ptr<ContactDataMultiple>& data,
171 const MatrixXs& ddv_dx)
const {
172 if (static_cast<std::size_t>(ddv_dx.rows()) != state_->get_nv() ||
173 static_cast<std::size_t
>(ddv_dx.cols()) != state_->get_ndx()) {
174 throw_pretty(
"Invalid argument: " 175 <<
"ddv_dx has wrong dimension (it should be " + std::to_string(state_->get_nv()) +
"," +
176 std::to_string(state_->get_ndx()) +
")");
178 data->ddv_dx = ddv_dx;
181 template <
typename Scalar>
182 void ContactModelMultipleTpl<Scalar>::updateForceDiff(
const boost::shared_ptr<ContactDataMultiple>& data,
183 const MatrixXs& df_dx,
const MatrixXs& df_du)
const {
184 const std::size_t& ndx = state_->get_ndx();
185 if (static_cast<std::size_t>(df_dx.rows()) != nc_ || static_cast<std::size_t>(df_dx.cols()) != ndx) {
186 throw_pretty(
"Invalid argument: " 187 <<
"df_dx has wrong dimension (it should be " + std::to_string(nc_) +
"," + std::to_string(ndx) +
190 if (static_cast<std::size_t>(df_du.rows()) != nc_ || static_cast<std::size_t>(df_du.cols()) != nu_) {
191 throw_pretty(
"Invalid argument: " 192 <<
"df_du has wrong dimension (it should be " + std::to_string(nc_) +
"," + std::to_string(nu_) +
195 if (static_cast<std::size_t>(data->contacts.size()) != contacts_.size()) {
196 throw_pretty(
"Invalid argument: " 197 <<
"it doesn't match the number of contact datas and models");
201 typename ContactModelContainer::const_iterator it_m, end_m;
202 typename ContactDataContainer::const_iterator it_d, end_d;
203 for (it_m = contacts_.begin(), end_m = contacts_.end(), it_d = data->contacts.begin(), end_d = data->contacts.end();
204 it_m != end_m || it_d != end_d; ++it_m, ++it_d) {
205 const boost::shared_ptr<ContactItem>& m_i = it_m->second;
207 const boost::shared_ptr<ContactDataAbstract>& d_i = it_d->second;
208 assert_pretty(it_m->first == it_d->first,
"it doesn't match the contact name between data and model");
210 std::size_t
const& nc_i = m_i->contact->get_nc();
211 const Eigen::Block<const MatrixXs> df_dx_i = df_dx.block(nc, 0, nc_i, ndx);
212 const Eigen::Block<const MatrixXs> df_du_i = df_du.block(nc, 0, nc_i, nu_);
213 m_i->contact->updateForceDiff(d_i, df_dx_i, df_du_i);
219 template <
typename Scalar>
220 boost::shared_ptr<ContactDataMultipleTpl<Scalar> > ContactModelMultipleTpl<Scalar>::createData(
221 pinocchio::DataTpl<Scalar>*
const data) {
222 return boost::make_shared<ContactDataMultiple>(
this, data);
225 template <
typename Scalar>
226 const boost::shared_ptr<StateMultibodyTpl<Scalar> >& ContactModelMultipleTpl<Scalar>::get_state()
const {
230 template <
typename Scalar>
231 const typename ContactModelMultipleTpl<Scalar>::ContactModelContainer& ContactModelMultipleTpl<Scalar>::get_contacts()
236 template <
typename Scalar>
237 const std::size_t& ContactModelMultipleTpl<Scalar>::get_nc()
const {
241 template <
typename Scalar>
242 const std::size_t& ContactModelMultipleTpl<Scalar>::get_nc_total()
const {
246 template <
typename Scalar>
247 const std::size_t& ContactModelMultipleTpl<Scalar>::get_nu()
const {
Definition: action-base.hxx:11