CollisionFunction.h

Go to the documentation of this file.

 
#pragma once
 
#include <tvm/api.h>
#include <tvm/defs.h>
 
#include <tvm/function/abstract/Function.h>
#include <tvm/robot/ConvexHull.h>
 
namespace tvm
{
 
namespace robot
{
 
class TVM_DLLAPI CollisionFunction : public function::abstract::Function
{
public:
  SET_UPDATES(CollisionFunction, Value, Velocity, Jacobian, Time, NormalAcceleration)
 
  
  CollisionFunction(Clock & clock);
 
  void addCollision(ConvexHullPtr ch1, ConvexHullPtr ch2);
 
  void reset();
 
protected:
  /* Update functions */
  void updateValue();
  void updateVelocity();
  void updateJacobian();
  void updateTimeDependency();
  void updateNormalAcceleration();
 
  Clock & clock_;
  uint64_t last_tick_ = 0;
 
  struct CollisionData
  {
    struct ObjectData
    {
      Eigen::Vector3d nearestPoint_; // In body coordinates
      rbd::Jacobian jac_;
    };
    std::vector<ObjectData> objects_;
    ConvexHullPtr ch_[2];
    sch::CD_Pair pair_;
    Eigen::Vector3d normVecDist_;
    Eigen::Vector3d prevNormVecDist_ = Eigen::Vector3d::Zero();
    Eigen::Vector3d speedVec_ = Eigen::Vector3d::Zero();
 
    CollisionData();
 
    CollisionData(CollisionFunction & fn, ConvexHullPtr ch1, ConvexHullPtr ch2);
  };
  std::vector<CollisionData> colls_;
 
  Eigen::Vector3d closestPoints_[2];
  Eigen::MatrixXd fullJac_;
  Eigen::MatrixXd distJac_;
};
 
} // namespace robot
 
} // namespace tvm