33 virtual void lockJoint(
unsigned int q_nr,
unsigned int ndof)
35 q_nr += m_qrange.
start;
36 project(m_scene->m_Wq,
Range(q_nr, ndof), m_qrange).setZero();
39 virtual void lockJoint(
unsigned int q_nr,
unsigned int ndof,
double *qdot)
41 q_nr += m_qrange.
start;
42 project(m_scene->m_Wq,
Range(q_nr, ndof), m_qrange).setZero();
46 for (
unsigned int i = 0; i < ndof; ++i, ++q_nr) {
47 m_scene->m_ydot -= m_scene->m_A.col(q_nr) * qdot[i];
84 ConstraintMap::iterator constraint_it;
85 while ((constraint_it = constraints.begin()) != constraints.end()) {
86 delete constraint_it->second;
87 constraints.erase(constraint_it);
89 ObjectMap::iterator object_it;
90 while ((object_it = objects.begin()) != objects.end()) {
91 delete object_it->second;
92 objects.erase(object_it);
114 const std::string &baseFrame)
122 if (baseFrameIndex < 0)
124 std::pair<ObjectMap::iterator, bool>
result;
127 result = objects.insert(ObjectMap::value_type(
129 new Object_struct(
object,
138 ObjectMap::iterator base_it;
139 for (base_it = objects.begin(); base_it != objects.end(); base_it++) {
140 if (base_it->second->object == base)
143 if (base_it == objects.end())
145 result = objects.insert(ObjectMap::value_type(
147 new Object_struct(
object,
152 base_it->second->coordinaterange)));
157 m_nqTotal +=
object->getNrOfCoordinates();
164 std::pair<ObjectMap::iterator, bool>
result = objects.insert(
165 ObjectMap::value_type(name,
166 new Object_struct(
object,
174 m_nuTotal +=
object->getNrOfCoordinates();
182 const std::string &object1,
183 const std::string &object2,
184 const std::string &ee1,
185 const std::string &ee2)
188 ObjectMap::iterator object1_it = objects.find(object1);
189 ObjectMap::iterator object2_it = objects.find(object2);
190 if (object1_it == objects.end() || object2_it == objects.end())
192 int ee1_index = object1_it->second->object->addEndEffector(ee1);
193 int ee2_index = object2_it->second->object->addEndEffector(ee2);
194 if (ee1_index < 0 || ee2_index < 0)
196 std::pair<ConstraintMap::iterator, bool>
result = constraints.insert(ConstraintMap::value_type(
198 new ConstraintSet_struct(
task,
203 Range(m_ncTotal,
task->getNrOfConstraints()),
204 Range(6 * m_nsets, 6))));
207 m_ncTotal +=
task->getNrOfConstraints();
214 if (m_solver ==
NULL) {
224 if (m_cache ==
NULL) {
236 if (m_ncTotal == 0 || m_nqTotal == 0 || m_nsets == 0)
258 for (ObjectMap::iterator it = objects.begin(); it != objects.end(); ++it) {
259 Object_struct *os = it->second;
261 os->object->initCache(m_cache);
262 if (os->constraintrange.count > 0)
268 m_ytask.resize(m_ncTotal);
272 for (ConstraintMap::iterator it = constraints.begin(); it != constraints.end(); ++it) {
274 ConstraintSet_struct *cs = it->second;
276 getConstraintPose(cs->task, cs, external_pose);
277 result &= cs->task->initialise(external_pose);
278 cs->task->initCache(m_cache);
279 for (
int i = 0; i < cs->constraintrange.count; i++,
count++) {
280 m_ytask[
count] = toggle;
283 project(m_Cf, cs->constraintrange, cs->featurerange) = cs->task->getCf();
286 if (m_solver !=
NULL)
287 m_solver->
init(m_nqTotal, m_ncTotal, m_ytask);
297 ConstraintSet_struct *cs = (ConstraintSet_struct *)_param;
299 assert(constraint == cs->task);
300 Object_struct *ob1 = cs->object1->second;
301 Object_struct *ob2 = cs->object2->second;
304 (ob1->base->getPose(ob1->baseFrameIndex) * ob1->object->getPose(cs->ee1index)).Inverse() *
305 (ob2->base->getPose(ob2->baseFrameIndex) * ob2->object->getPose(cs->ee2index));
311 unsigned int numsubstep,
330 ts.
cache = (cache) ? 1 : 0;
332 ts.
numstep = (numsubstep & 0xFF);
333 bool autosubstep = (numsubstep == 0) ?
true :
false;
336 double timesubstep = timestep / numsubstep;
337 double timeleft = timestep;
339 if (timeleft == 0.0) {
341 for (ObjectMap::iterator it = objects.begin(); it != objects.end(); ++it) {
342 it->second->object->pushCache(ts);
345 for (ConstraintMap::iterator it = constraints.begin(); it != constraints.end(); ++it) {
346 it->second->task->pushCache(ts);
358 while (numsubstep > 0) {
360 for (ObjectMap::iterator it = objects.begin(); it != objects.end(); ++it) {
361 Object_struct *os = it->second;
364 if (os->constraintrange.count > 0) {
366 os->constraintrange) = ((
ControlledObject *)(os->object))->getControlOutput();
371 if (os->jointrange.count > 0) {
373 m_Wq, os->jointrange, os->jointrange) = ((
ControlledObject *)(os->object))->getWq();
387 for (ConstraintMap::iterator it = constraints.begin(); it != constraints.end(); ++it) {
388 ConstraintSet_struct *cs = it->second;
389 Object_struct *ob1 = cs->object1->second;
390 Object_struct *ob2 = cs->object2->second;
392 if (ob1->base->updated() || ob1->object->updated() || ob2->base->updated() ||
393 ob2->object->updated()) {
396 getConstraintPose(cs->task, cs, external_pose);
397 cs->task->initialise(external_pose);
399 cs->task->updateControlOutput(ts);
400 project(m_ydot, cs->constraintrange) = cs->task->getControlOutput();
401 if (!ts.
substep || cs->task->substep()) {
402 project(m_Wy, cs->constraintrange) = (cs->task)->getWy();
406 project(m_Jf, cs->featurerange, cs->featurerange) = cs->task->getJf();
409 Eigen::Block<e_matrix> Jf_part =
project(m_Jf, cs->featurerange, cs->featurerange);
411 ob1->base->getPose(ob1->baseFrameIndex) * ob1->object->getPose(cs->ee1index));
416 project(m_Jf, cs->featurerange, cs->featurerange), m_Uf, m_Sf, m_Vf, m_tempf);
417 for (
unsigned int i = 0; i < 6; ++i)
419 m_Uf.col(i).setConstant(0.0);
421 m_Uf.col(i) *= (1 / m_Sf(i));
422 project(m_Jf_inv, cs->featurerange, cs->featurerange).noalias() = m_Vf * m_Uf.transpose();
430 ob1->jointrange) = (((
ControlledObject *)(ob1->object))->getJq(cs->ee1index));
432 Eigen::Block<e_matrix> Jq_part =
project(m_Jq, cs->featurerange, ob1->jointrange);
433 changeBase(Jq_part, ob1->base->getPose(ob1->baseFrameIndex));
435 if (ob1->base->getNrOfCoordinates() != 0) {
437 project(m_Ju, cs->featurerange, ob1->coordinaterange) = ob1->base->getJu(
438 ob1->baseFrameIndex);
452 if (ob1->object == ob2->object) {
456 changeBase(JqTemp, ob2->base->getPose(ob2->baseFrameIndex));
458 project(m_Jq, cs->featurerange, ob2->jointrange) -= JqTemp;
461 project(m_Jq, cs->featurerange, ob2->jointrange) = -(
464 Eigen::Block<e_matrix> Jq_part =
project(m_Jq, cs->featurerange, ob2->jointrange);
465 changeBase(Jq_part, ob2->base->getPose(ob2->baseFrameIndex));
467 if (ob2->base->getNrOfCoordinates() != 0) {
470 if (ob2->base == ob1->base || ob2->base == ob1->object) {
471 project(m_Ju, cs->featurerange, ob2->coordinaterange) -= ob2->base->getJu(
472 ob2->baseFrameIndex);
475 project(m_Ju, cs->featurerange, ob2->coordinaterange) = -ob2->base->getJu(
476 ob2->baseFrameIndex);
482 if (ob2->object == ob1->base || ob2->object == ob1->object) {
483 project(m_Ju, cs->featurerange, ob2->coordinaterange) -=
487 project(m_Ju, cs->featurerange, ob2->coordinaterange) =
494 m_Atemp.noalias() = m_Cf * m_Jf_inv;
495 m_A.noalias() = m_Cq - (m_Atemp * m_Jq);
497 m_B.noalias() = m_Atemp * m_Ju;
498 m_ydot.noalias() += m_B * m_xdot;
502 if (!m_solver->
solve(m_A, m_Wy, m_ydot, m_Wq, m_qdot, nlcoef))
506 for (ObjectMap::iterator it = objects.begin(); it != objects.end(); ++it) {
507 Object_struct *os = it->second;
512 for (ConstraintMap::iterator it = constraints.begin(); it != constraints.end(); ++it) {
513 ConstraintSet_struct *cs = it->second;
514 Object_struct *ob1 = cs->object1->second;
515 Object_struct *ob2 = cs->object2->second;
518 if (ob1->jointrange.count > 0)
519 external_vel.noalias() += (
project(m_Jq, cs->featurerange, ob1->jointrange) *
520 project(m_qdot, ob1->jointrange));
521 if (ob2->jointrange.count > 0)
522 external_vel.noalias() += (
project(m_Jq, cs->featurerange, ob2->jointrange) *
523 project(m_qdot, ob2->jointrange));
524 if (ob1->coordinaterange.count > 0)
525 external_vel.noalias() += (
project(m_Ju, cs->featurerange, ob1->coordinaterange) *
526 project(m_xdot, ob1->coordinaterange));
527 if (ob2->coordinaterange.count > 0)
528 external_vel.noalias() += (
project(m_Ju, cs->featurerange, ob2->coordinaterange) *
529 project(m_xdot, ob2->coordinaterange));
532 e_vector6 estimated_chidot =
project(m_Jf_inv, cs->featurerange, cs->featurerange) *
534 cs->task->setJointVelocity(estimated_chidot);
541 timesubstep = timeleft;
544 double maxsubstep = nlcoef * m_maxstep;
545 if (maxsubstep < m_minstep)
546 maxsubstep = m_minstep;
547 if (timesubstep > maxsubstep)
548 timesubstep = maxsubstep;
549 for (ObjectMap::iterator it = objects.begin(); it != objects.end(); ++it) {
550 Object_struct *os = it->second;
554 for (ConstraintMap::iterator it = constraints.begin(); it != constraints.end(); ++it) {
555 ConstraintSet_struct *cs = it->second;
556 cs->task->getMaxTimestep(timesubstep);
559 maxsubstep = 2.0 *
floor(timestep / 2.0 / timesubstep - 0.66666);
560 timesubstep = (maxsubstep < 0.0) ? timestep : timestep / (2.0 + maxsubstep);
561 if (timesubstep >= timeleft - (m_minstep / 2.0)) {
562 timesubstep = timeleft;
568 timeleft -= timesubstep;
571 if (numsubstep > 1) {
584 ObjectMap::iterator it;
587 for (it = objects.begin(); it != objects.end(); ++it) {
590 lockCallback.
setRange(os->jointrange);
601 if (!m_solver->
solve(m_A, m_Wy, m_ydot, m_Wq, m_qdot, nlcoef))
606 for (it = objects.begin(); it != objects.end(); ++it) {
615 for (ObjectMap::iterator it = objects.begin(); it != objects.end(); ++it) {
616 it->second->object->updateKinematics(ts);
619 it->second->object->updated(
false);
622 for (ConstraintMap::iterator it = constraints.begin(); it != constraints.end(); ++it) {
623 ConstraintSet_struct *cs = it->second;
625 getConstraintPose(cs->task, cs, external_pose);
626 cs->task->modelUpdate(external_pose, ts);
628 cs->task->updateKinematics(ts);
represents a frame transformation in 3D space (rotation + translation)
virtual const unsigned int getNrOfCoordinates()
virtual const ObjectType getType()
virtual int addEndEffector(const std::string &name)
virtual void lockJoint(unsigned int q_nr, unsigned int ndof, double *qdot)
void setRange(Range &range)
virtual void lockJoint(unsigned int q_nr, unsigned int ndof)
bool addSolver(Solver *_solver)
bool setParam(SceneParam paramId, double value)
bool addConstraintSet(const std::string &name, ConstraintSet *task, const std::string &object1, const std::string &object2, const std::string &ee1="", const std::string &ee2="")
bool update(double timestamp, double timestep, unsigned int numsubstep=1, bool reiterate=false, bool cache=true, bool interpolate=true)
bool addCache(Cache *_cache)
bool addObject(const std::string &name, Object *object, UncontrolledObject *base=&Object::world, const std::string &baseFrame="")
virtual bool init(unsigned int nq, unsigned int nc, const std::vector< bool > &gc)=0
virtual bool solve(const e_matrix &A, const e_vector &Wy, const e_vector &ydot, const e_matrix &Wq, e_vector &qdot, e_scalar &nlcoef)=0
virtual const unsigned int getNrOfCoordinates()
double epsilon
default precision while comparing with Equal(..,..) functions. Initialized at 0.0000001.
int svd_eigen_HH(const Eigen::MatrixBase< MatrixA > &A, Eigen::MatrixBase< MatrixUV > &U, Eigen::MatrixBase< VectorS > &S, Eigen::MatrixBase< MatrixUV > &V, Eigen::MatrixBase< VectorS > &tmp, int maxiter=150)
struct blender::compositor::@179::@181 task
void interpolate(const Span< T > src, const Span< int > indices, const Span< float > factors, MutableSpan< T > dst)
void setCacheTimestamp(Timestamp ×tamp)
static int changeBase(Eigen::MatrixBase< Derived > &J, const Frame &T)
Eigen::Block< MatrixType > project(MatrixType &m, Range r)