39 eps_ = 1.0 / init.Precision;
40 for (std::size_t i = 0; i < init.EndEffector.size(); ++i)
42 SphereInitializer sphere(init.EndEffector[i]);
43 groups_[sphere.Group].push_back(i);
45 visualization_msgs::Marker mrk;
46 mrk.action = visualization_msgs::Marker::ADD;
47 mrk.header.frame_id = init.ReferenceFrame;
49 mrk.type = visualization_msgs::Marker::SPHERE;
50 mrk.scale.x = mrk.scale.y = mrk.scale.z = sphere.Radius * 2;
56 col.a =
static_cast<float>(init.Alpha);
57 for (
int i : it.second)
66 debug_pub_ = Server::Advertise<visualization_msgs::MarkerArray>(
ns_ +
"/CollisionSpheres", 1,
true);
69 dim_ = groups_.size() * (groups_.size() - 1) / 2;
74 return 1.0 / (1.0 +
exp(5.0 *
eps_ * ((eff_A.
p - eff_B.
p).Norm() - r_A - r_B)));
79 const Eigen::Vector3d eA_minus_eB = Eigen::Map<const Eigen::Vector3d>(eff_A.
p.
data) - Eigen::Map<const Eigen::Vector3d>(eff_B.
p.
data);
80 const double eA_minus_eB_norm = eA_minus_eB.norm();
81 for (
unsigned int i = 0; i < jacA.
columns(); ++i)
83 ret(i) += -
static_cast<double>((jacA.
data.col(i).head<3>() - jacB.
data.col(i).head<3>()).adjoint() * eA_minus_eB) / eA_minus_eB_norm;
97 for (
auto B = std::next(
A); B != Bend; ++B)
99 for (std::size_t ii = 0; ii <
A->second.size(); ++ii)
101 for (std::size_t jj = 0; jj < B->second.size(); ++jj)
103 int i =
A->second[ii];
104 int j = B->second[jj];
114 for (std::size_t i = 0; i <
debug_msg_.markers.size(); ++i)
134 for (
auto A =
groups_.begin();
A != Aend; ++
A)
136 for (
auto B = std::next(
A); B != Bend; ++B)
138 for (std::size_t ii = 0; ii <
A->second.size(); ++ii)
140 for (std::size_t jj = 0; jj < B->second.size(); ++jj)
142 int i =
A->second[ii];
143 int j = B->second[jj];
154 for (std::size_t i = 0; i <
debug_msg_.markers.size(); ++i)
void publish(const boost::shared_ptr< M > &message) const
std_msgs::ColorRGBA RandomColor()
double Distance(const KDL::Frame &eff_A, const KDL::Frame &eff_B, double r_A, double r_B)
Eigen::Ref< Eigen::VectorXd > VectorXdRef
ros::Publisher debug_pub_
unsigned int columns() const
EIGEN_MAKE_ALIGNED_OPERATOR_NEW Eigen::Matrix< double, 6, Eigen::Dynamic > data
std::vector< double > radiuses_
void Instantiate(const SphereCollisionInitializer &init) override
void Jacobian(const KDL::Frame &eff_A, const KDL::Frame &eff_B, const KDL::Jacobian &jacA, const KDL::Jacobian &jacB, double r_A, double r_B, Eigen::Block< Eigen::Ref< Eigen::MatrixXd >, 1,-1, false > ret)
int TaskSpaceDim() override
void Update(Eigen::VectorXdRefConst x, Eigen::VectorXdRef phi) override
const Eigen::Ref< const Eigen::VectorXd > & VectorXdRefConst
INLINE Rall1d< T, V, S > exp(const Rall1d< T, V, S > &arg)
REGISTER_TASKMAP_TYPE("SphereCollision", exotica::SphereCollision)
std::map< std::string, std::vector< int > > groups_
Eigen::Ref< Eigen::MatrixXd > MatrixXdRef
visualization_msgs::MarkerArray debug_msg_