54 if (!
scene_->AlwaysUpdatesCollisionScene())
55 cscene_->UpdateCollisionObjectTransforms();
58 for (
int i = 0; i <
dim_; ++i)
61 if (proxies.size() == 0)
69 closest_proxy.
distance = std::numeric_limits<double>::max();
70 for (
const auto& tmp_proxy : proxies)
72 const bool is_robot_to_robot = (tmp_proxy.e1->is_robot_link || tmp_proxy.e1->closest_robot_link.lock()) && (tmp_proxy.e2->is_robot_link || tmp_proxy.e2->closest_robot_link.lock());
74 if ((tmp_proxy.distance - margin) < closest_proxy.
distance)
76 closest_proxy = tmp_proxy;
90 Eigen::MatrixXd tmpJ =
scene_->GetKinematicTree().Jacobian(
92 J.row(i) += (closest_proxy.
normal1.transpose() * tmpJ.topRows<3>());
93 tmpJ =
scene_->GetKinematicTree().Jacobian(closest_proxy.
e2, brel,
nullptr,
95 J.row(i) -= (closest_proxy.
normal1.transpose() * tmpJ.topRows<3>());
CollisionScenePtr cscene_
std::vector< std::string > robot_joints_
bool check_self_collision_
int TaskSpaceDim() override
void AssignScene(ScenePtr scene) override
Eigen::Ref< Eigen::VectorXd > VectorXdRef
std::vector< CollisionProxy > closest_proxies_
void Update(Eigen::VectorXdRefConst x, Eigen::VectorXdRef phi) override
REGISTER_TASKMAP_TYPE("CollisionDistance", exotica::CollisionDistance)
std::shared_ptr< Scene > ScenePtr
void UpdateInternal(Eigen::VectorXdRefConst x, Eigen::VectorXdRef phi, Eigen::MatrixXdRef J, bool updateJacobian=true)
CollisionDistanceInitializer parameters_
const Eigen::Ref< const Eigen::VectorXd > & VectorXdRefConst
#define HIGHLIGHT_NAMED(name, x)
Eigen::Ref< Eigen::MatrixXd > MatrixXdRef
std::map< std::string, std::vector< std::string > > controlled_joint_to_collision_link_map_
std::shared_ptr< KinematicElement > e2
std::shared_ptr< KinematicElement > e1