void CollisionSpace::drawCollisionPoints() {
for (unsigned int i = 0; i < m_Robot->getNumberOfJoints(); i++) {
Joint* jnt = m_Robot->getJoint(i);
Eigen::Transform3d T = jnt->getMatrixPos();
std::vector<CollisionPoint>& points = m_sampler->getCollisionPoints(jnt);
for (unsigned int j = 0; j < points.size(); j++) {
// if (points[j].getSegmentNumber() <ENV.getDouble(Env::extensionStep))
// continue;
if (points[j].m_is_colliding) {
double color[4];
color[1] = 0.0; // green
color[2] = 0.0; // blue
color[0] = 1.0; // red
color[3] = 0.7; // transparency
g3d_set_color(Any, color);
}
bool yellow = (!points[j].m_is_colliding);
points[j].draw(T, yellow);
}
}
}
double CollisionSpace::cost(const Configuration& q) const {
// bool colliding = false;
double distance = 0.0;
double potential = 0.0;
// colliding =
// isRobotColliding( distance, potential );
double cost = 0.0;
Eigen::Vector3d position, gradient;
for (unsigned int i = 0; i < m_Robot->getNumberOfJoints(); i++) {
Joint* jnt = m_Robot->getJoint(i);
std::vector<CollisionPoint>& points = m_sampler->getCollisionPoints(jnt);
if (points.empty()) continue;
Eigen::Transform3d T = jnt->getMatrixPos();
for (unsigned int j = 0; j < points.size(); j++) {
position = T * points[j].getPosition();
getCollisionPointPotentialGradient(
points[j], position, distance, potential, gradient);
if (potential < EPS6) {
potential = EPS6;
}
cost += potential;
}
}
return 10 * cost;
}
void CollisionSpace::addEnvPoints() {
// Add Static Obstacles
for (int i = 0; i < XYZ_ENV->no; i++) {
PointCloud& cloud = m_sampler->getPointCloud(XYZ_ENV->o[i]);
for (unsigned int j = 0; j < cloud.size(); j++)
m_points_to_add.push_back(cloud[j]);
}
// Add Moving Obstacles
Scene* sc = global_Project->getActiveScene();
for (unsigned int i = 0; i < sc->getNumberOfRobots(); i++) {
Robot* mov_obst = sc->getRobot(i);
// The robot is not a robot or a human
if (!((mov_obst->getName().find("ROBOT") != std::string::npos) ||
(mov_obst->getName().find("HUMAN") != std::string::npos) ||
mov_obst->getName() == "rob1" || mov_obst->getName() == "rob2" ||
mov_obst->getName() == "rob3" || mov_obst->getName() == "rob4")) {
cout << "Adding : " << mov_obst->getName() << endl;
Joint* jnt = mov_obst->getJoint(1);
if (static_cast<p3d_jnt*>(jnt->getP3dJointStruct())->o == NULL) continue;
PointCloud& cloud = m_sampler->getPointCloud(
static_cast<p3d_jnt*>(jnt->getP3dJointStruct())->o);
for (int j = 0; j < int(cloud.size()); j++) {
m_points_to_add.push_back(jnt->getMatrixPos() * cloud[j]);
}
}
}
}
void Boxes::drawCollisionPoints()
{
for( size_t i=0; i<active_joints_.size(); i++ )
{
Joint* jnt = robot_->getJoint( active_joints_[i] );
std::vector<CollisionPoint>& points = sampler_->getCollisionPoints(jnt);
Eigen::Transform3d T = jnt->getMatrixPos();
cout << "joint : " << active_joints_[i] << " , points size : " << points.size() << endl;
for( size_t j=0; j<points.size(); j++ )
{
if( points[j].m_is_colliding )
{
double color[4];
color[0] = 1.0; // (r)ed
color[1] = 0.0; // (g)reen
color[2] = 0.0; // (b)lue
color[3] = 0.7; // transparency
g3d_set_color(Any,color);
}
bool yellow = true;
//bool yellow = (!points[j].m_is_colliding);
points[j].draw( T, yellow );
}
}
}
//! Goes through all points and computes whether the robot is colliding
//! Computes the transform of all point from the joint transform
//! Uses the potential gradient function
bool CollisionSpace::isRobotColliding(double& dist, double& pot) const {
// cout << "Test collision space is robot colliding" << endl;
bool isRobotColliding = false;
double potential = 0.0;
double max_potential = -std::numeric_limits<double>::max();
double distance = 0.0;
double min_distance = std::numeric_limits<double>::max();
Eigen::Vector3d position, gradient;
for (unsigned int i = 0; i < m_Robot->getNumberOfJoints(); i++) {
Joint* jnt = m_Robot->getJoint(i);
std::vector<CollisionPoint>& points = m_sampler->getCollisionPoints(jnt);
if (points.empty()) continue;
Eigen::Transform3d T = jnt->getMatrixPos();
for (unsigned int j = 0; j < points.size(); j++) {
position = T * points[j].getPosition();
// CollisionSpaceCell* cell = static_cast<CollisionSpaceCell*>( getCell(
// position ) );
// if( ( cell != NULL ? getDistance( cell ) : 0 ) <=
// points[j].getRadius()
// )
if (getCollisionPointPotentialGradient(
points[j], position, distance, potential, gradient)) {
points[j].m_is_colliding = true;
//cout << "point[" << j
// << "] in joint is in collision: " << points[j].joint()->getName()
// << endl;
// Hack!!! to exclude certain points
if (points[j].getSegmentNumber() > 1) {
// cout << "points[" << j << "].getSegmentNumber()" << endl;
isRobotColliding = true;
}
} else {
points[j].m_is_colliding = false;
}
if (max_potential < potential) max_potential = potential;
if (min_distance > distance) min_distance = distance;
}
}
pot = max_potential;
dist = min_distance;
return isRobotColliding;
}