//////////////////////////////////////////////////
// INLINE IMPLEMENTATION
//////////////////////////////////////////////////
template < typename _Potential > inline void
FixedPairListTypesInteractionTemplate < _Potential >::addForces() {
LOG4ESPP_INFO(theLogger, "add forces computed by the FixedPair List");
const bc::BC& bc = *getSystemRef().bc;
for (FixedPairList::PairList::Iterator it(*fixedpairList); it.isValid(); ++it) {
Particle &p1 = *it->first;
Particle &p2 = *it->second;
int type1 = p1.type();
int type2 = p2.type();
const Potential &potential = getPotential(type1, type2);
// shared_ptr<Potential> potential = getPotential(type1, type2);
Real3D force(0.0);
//if(potential._computeForce(force, p1, p2)) {
////if(potential->_computeForce(force, p1, p2)) {
// p1.force() += force;
// p2.force() -= force;
// LOG4ESPP_TRACE(theLogger, "id1=" << p1.id() << " id2=" << p2.id() << " force=" << force);
//}
Real3D dist;
bc.getMinimumImageVectorBox(dist, p1.position(), p2.position());
if(potential._computeForce(force, p1, p2, dist)) {
p1.force() += force;
p2.force() -= force;
}
}
}
inline real
FixedPairListTypesInteractionTemplate < _Potential >::
computeEnergy() {
LOG4ESPP_INFO(theLogger, "compute energy of the FixedPair list pairs");
real e = 0.0;
real es = 0.0;
const bc::BC& bc = *getSystemRef().bc; // boundary conditions
for (FixedPairList::PairList::Iterator it(*fixedpairList);
it.isValid(); ++it) {
Particle &p1 = *it->first;
Particle &p2 = *it->second;
int type1 = p1.type();
int type2 = p2.type();
const Potential &potential = getPotential(type1, type2);
// shared_ptr<Potential> potential = getPotential(type1, type2);
Real3D r21;
bc.getMinimumImageVectorBox(r21, p1.position(), p2.position());
//e = potential._computeEnergy(p1, p2);
// e = potential->_computeEnergy(p1, p2);
e = potential._computeEnergy(p1,p2,r21);
es += e;
LOG4ESPP_TRACE(theLogger, "id1=" << p1.id() << " id2=" << p2.id() << " potential energy=" << e);
//std::cout << "id1=" << p1.id() << " id2=" << p2.id() << " potential energy=" << e << std::endl;
}
// reduce over all CPUs
real esum;
boost::mpi::all_reduce(*mpiWorld, es, esum, std::plus<real>());
return esum;
}
inline real
FixedQuadrupleListTypesInteractionTemplate<_DihedralPotential>::
computeEnergy() {
LOG4ESPP_INFO(theLogger, "compute energy of the quadruples");
const bc::BC &bc = *getSystemRef().bc; // boundary conditions
real e = 0.0;
for (FixedQuadrupleList::QuadrupleList::Iterator it(*fixedquadrupleList); it.isValid(); ++it) {
const Particle &p1 = *it->first;
const Particle &p2 = *it->second;
const Particle &p3 = *it->third;
const Particle &p4 = *it->fourth;
longint type1 = p1.type();
longint type2 = p2.type();
longint type3 = p3.type();
longint type4 = p4.type();
const Potential &potential = getPotential(type1, type2, type3, type4);
Real3D dist21, dist32, dist43;
bc.getMinimumImageVectorBox(dist21, p2.position(), p1.position());
bc.getMinimumImageVectorBox(dist32, p3.position(), p2.position());
bc.getMinimumImageVectorBox(dist43, p4.position(), p3.position());
e += potential.computeEnergy(dist21, dist32, dist43);
}
real esum;
boost::mpi::all_reduce(*mpiWorld, e, esum, std::plus<real>());
return esum;
}
inline real
FixedPairListTypesInteractionTemplate< _Potential >::getMaxCutoff() {
real cutoff = 0.0;
for (int i = 0; i < ntypes; i++) {
for (int j = 0; j < ntypes; j++) {
cutoff = std::max(cutoff, getPotential(i, j).getCutoff());
// cutoff = std::max(cutoff, getPotential(i, j)->getCutoff());
}
}
return cutoff;
}
inline real
FixedTripleListTypesInteractionTemplate<_Potential>::getMaxCutoff() {
real cutoff = 0.0;
for (int i = 0; i < ntypes; i++) {
for (int j = 0; j < ntypes; j++) {
for (int k = 0; k < ntypes; k++) {
cutoff = std::max(cutoff, getPotential(i, j, k).getCutoff());
}
}
}
return cutoff;
}
inline void
FixedQuadrupleListTypesInteractionTemplate<_DihedralPotential>::
computeVirialTensor(Tensor &w, real z) {
LOG4ESPP_INFO(theLogger, "compute the virial tensor of the quadruples");
Tensor wlocal(0.0);
const bc::BC &bc = *getSystemRef().bc;
std::cout << "Warning!!! computeVirialTensor in specified volume doesn't work for "
"FixedQuadrupleListTypesInteractionTemplate at the moment" << std::endl;
for (FixedQuadrupleList::QuadrupleList::Iterator it(*fixedquadrupleList); it.isValid(); ++it) {
const Particle &p1 = *it->first;
const Particle &p2 = *it->second;
const Particle &p3 = *it->third;
const Particle &p4 = *it->fourth;
longint type1 = p1.type();
longint type2 = p2.type();
longint type3 = p3.type();
longint type4 = p4.type();
const Potential &potential = getPotential(type1, type2, type3, type4);
Real3D dist21, dist32, dist43;
bc.getMinimumImageVectorBox(dist21, p2.position(), p1.position());
bc.getMinimumImageVectorBox(dist32, p3.position(), p2.position());
bc.getMinimumImageVectorBox(dist43, p4.position(), p3.position());
Real3D force1, force2, force3, force4;
potential.computeForce(force1, force2, force3, force4,
dist21, dist32, dist43);
// TODO: formulas are not correct yet
wlocal += Tensor(dist21, force1) - Tensor(dist32, force2);
}
// reduce over all CPUs
Tensor wsum(0.0);
boost::mpi::all_reduce(*mpiWorld, (double *) &wlocal, 6, (double *) &wsum, std::plus<double>());
w += wsum;
}
inline real
FixedQuadrupleListTypesInteractionTemplate<_DihedralPotential>::
computeVirial() {
LOG4ESPP_INFO(theLogger, "compute scalar virial of the quadruples");
real w = 0.0;
const bc::BC &bc = *getSystemRef().bc; // boundary conditions
for (FixedQuadrupleList::QuadrupleList::Iterator it(*fixedquadrupleList); it.isValid(); ++it) {
const Particle &p1 = *it->first;
const Particle &p2 = *it->second;
const Particle &p3 = *it->third;
const Particle &p4 = *it->fourth;
longint type1 = p1.type();
longint type2 = p2.type();
longint type3 = p3.type();
longint type4 = p4.type();
const Potential &potential = getPotential(type1, type2, type3, type4);
Real3D dist21, dist32, dist43;
bc.getMinimumImageVectorBox(dist21, p2.position(), p1.position());
bc.getMinimumImageVectorBox(dist32, p3.position(), p2.position());
bc.getMinimumImageVectorBox(dist43, p4.position(), p3.position());
Real3D force1, force2, force3, force4;
potential.computeForce(force1, force2, force3, force4,
dist21, dist32, dist43);
// TODO: formulas are not correct yet?
w += dist21 * force1 + dist32 * force2;
}
real wsum;
boost::mpi::all_reduce(*mpiWorld, w, wsum, std::plus<real>());
return w;
}
inline void
FixedTripleListTypesInteractionTemplate<_Potential>::addForces() {
LOG4ESPP_INFO(theLogger, "add forces computed by the FixedTriple List");
const bc::BC &bc = *getSystemRef().bc;
for (FixedTripleList::TripleList::Iterator it(*fixedtripleList); it.isValid(); ++it) {
Particle &p1 = *it->first;
Particle &p2 = *it->second;
Particle &p3 = *it->third;
int type1 = p1.type();
int type2 = p2.type();
int type3 = p3.type();
const Potential &potential = getPotential(type1, type2, type3);
Real3D dist12, dist32;
bc.getMinimumImageVectorBox(dist12, p1.position(), p2.position());
bc.getMinimumImageVectorBox(dist32, p3.position(), p2.position());
Real3D force12, force32;
potential._computeForce(force12, force32, dist12, dist32);
p1.force() += force12;
p2.force() -= force12 + force32;
p3.force() += force32;
}
}
inline void
FixedQuadrupleListTypesInteractionTemplate<_DihedralPotential>::
addForces() {
LOG4ESPP_INFO(theLogger, "add forces computed by FixedQuadrupleList");
const bc::BC &bc = *getSystemRef().bc; // boundary conditions
for (FixedQuadrupleList::QuadrupleList::Iterator it(*fixedquadrupleList); it.isValid(); ++it) {
Particle &p1 = *it->first;
Particle &p2 = *it->second;
Particle &p3 = *it->third;
Particle &p4 = *it->fourth;
longint type1 = p1.type();
longint type2 = p2.type();
longint type3 = p3.type();
longint type4 = p4.type();
const Potential &potential = getPotential(type1, type2, type3, type4);
Real3D dist21, dist32, dist43;
bc.getMinimumImageVectorBox(dist21, p2.position(), p1.position());
bc.getMinimumImageVectorBox(dist32, p3.position(), p2.position());
bc.getMinimumImageVectorBox(dist43, p4.position(), p3.position());
Real3D force1, force2, force3, force4; // result forces
potential.computeForce(force1, force2, force3, force4,
dist21, dist32, dist43);
p1.force() += force1;
p2.force() += force2;
p3.force() += force3;
p4.force() += force4;
}
}
//! function to find a solution path
bool HFPlanner::trySolve()
{
//verify init and goal samples
if(goalSamp()->isFree()==false || initSamp()->isFree()==false)
{
cout<<"init or goal configuration are in COLLISION!"<<endl;
drawCspace();
return false;
}
//set init and goal vertices
gridVertex vg = _samples->indexOf(goalSamp());
gridVertex vi = _samples->indexOf(initSamp());
Sample* curr;
gridVertex vc;
gridVertex vmin;
curr = _init;
vc = vi;
_path.clear();
clearSimulationPath();
graph_traits<gridGraph>::adjacency_iterator avi, avi_end;
//relax HF
computeHF(vg);
int count = 0;
int countmax = _mainiter;
std::vector<int> cellpath;
cellpath.push_back(vi);
_path.push_back(locations[vi]);
while(vc != vg && count < countmax)
{
vmin = vc;
for(tie(avi,avi_end)=adjacent_vertices(vc, *g); avi!=avi_end; ++avi)
{
KthReal pneigh = getPotential(*avi);
KthReal pcurr = getPotential(vmin);
if(pneigh < pcurr) {
vmin = *avi;
cellpath.push_back(vmin);
_path.push_back(locations[vmin]);
}
}
if(vc == vmin) {
//relax HF again and resume
computeHF(vg);
_path.clear();
cellpath.clear();
vc = vi;
count++;
}
else vc = vmin;
}
if(count < countmax) _solved = true;
else
{
_path.clear();
cellpath.clear();
_solved = false;
}
if(_solved)
{
cout<<"PATH:";
for(int i=0;i<cellpath.size();i++)
{
cout<<" "<<cellpath[i]<<"("<<getPotential(cellpath[i])<<"), ";
}
cout<<endl;
}
drawCspace();
return _solved;
}
int CXXSpace::dumpSpaceSlice(int dimension, int gridType, int sliceNumber){
/* dumps a slice through space perpendicular to <dimension> with property of <type>. Space is sliced
at position <sliceNumber> along <dimension> axis. The dump is a textfile called dump.dat:
dimension:
0 x
1 y
3 z
looking at property:
type:
1 charge
2 dielectric value
3 potential
*/
// first make sure that the slice exists in the dimension specified
// make sure this direction is one of the three axis ..
int i, j, k;
// fstream dump ("dumpfile",ios::out|ios::app );
std::fstream dumper ("dumpfile.txt",ios::out|ios::app );
dumper.width(5);
dumper.precision(2);
//dumpXSlice(1,sliceNumber, 3);
if ((dimension > 2) | (dimension < 0)) {
CXXException theException("ERROR CXXSpace::dumpSpaceSlice() - dimension can only be 0,1 or 2!");
throw theException;
}
// make sure have that many slices in this direction
switch (dimension) {
case 0: // x-slice
if (sliceNumber > dim[0]) {
CXXException theException("ERROR CXXSpace::dumpSpaceSlice() 0 - outside of grid boundary ");
throw theException;
}
// now dump x slice...
dumper << "\nAtomGrid: Slicing through 3D grid at x= " << sliceNumber <<". Directions: (horizontal/vetical) <-> (z/y): \n\n";
for (k = 0; k < dim[2]; k++) {
for (j = 0; j < dim[1]; j++) {
switch (gridType) {
case 1:
// dump charge grid
dumper << getGridCharge(sliceNumber, j, k) << " ";
if (j == (dim[1]-1))
dumper << "\n\n";
break;
case 2:
// dump boundaryMap grid
dumper << setprecision(4) << getDielGrid(sliceNumber, j, k, 1) << " ";
if (j == (dim[1]-1))
dumper << "\n\n";
break;
case 3:
// dump potential - yet to come ...
dumper << setprecision(4) << getPotential(sliceNumber, j, k) << " ";
if (j == (dim[1]-1))
dumper << "\n\n";
break;
default:
CXXException theException("ERROR CXXSpace::dumpSpaceSlice() - No such grid !");
throw theException;
}
}
}
break;
// y-dimension
case 1: // y-slice
if (sliceNumber > dim[1]) {
CXXException theException("ERROR CXXSpace::dumpSpaceSlice() 1 - outside of grid boundary ");
throw theException;
}
dumper << "\nAtomGrid: Slicing through 3D grid at y= " << sliceNumber <<". Directions: (horizontal/vetical) <-> (x/z): \n\n";
// now dump y slice ...
for (k = 0; k < dim[2]; k++) {
for (i = 0; i < dim[0]; i++) {
switch (gridType) {
case 1:
// dump charge grid
dumper << setprecision(3) << getGridCharge(i, sliceNumber, k) << " ";
if (i == (dim[0]-1))
dumper << "\n\n";
break;
case 2:
// dump Boundary grid
dumper << setprecision(3) << getBoundaryMap(i, sliceNumber, k) << " ";
if (i == (dim[0]-1))
dumper << "\n\n";
break;
case 3:
// dump potential - yet to come ...
break;
default:
CXXException theException("ERROR CXXSpace::dumpSpaceSlice() - No such grid !");
throw theException;
}
}
}
break;
case 2: // z-slice
if (sliceNumber > dim[2]) {
CXXException theException("ERROR CXXSpace::dumpSpaceSlice() 2 - outside of grid boundary ");
throw theException;
}
// now dump z slice ...
dumper << "\nAtomGrid: Slicing through 3D grid at z= " << sliceNumber <<". Directions: (horizontal/vetical) <-> (x/y): \n\n";
for (j = 0; j < dim[1]; j++) {
for (i = 0; i < dim[0]; i++) {
switch (gridType) {
case 1:
// dump charge grid
dumper << setprecision(3) << getGridCharge(i, j, sliceNumber) << " ";
if (i == (dim[0]-1))
dumper << "\n\n";
break;
case 2:
// dump dielectric grid
dumper << "BoundaryMap dump\n\n";
dumper << setprecision(3) << getBoundaryMap(i, j, sliceNumber) << " ";
if (i == (dim[0]-1))
dumper << "\n\n";
break;
case 3:
// dump potential - yet to come ...
break;
default:
CXXException theException("ERROR CXXSpace::dumpSpaceSlice() - No such grid !");
throw theException;
}
}
}
break;
// end of outer switch clause - dimensiom
}
return 0;
}
void ChompOptimizer::performForwardKinematics()
{
double inv_time = 1.0 / group_trajectory_.getDiscretization();
double inv_time_sq = inv_time * inv_time;
// calculate the forward kinematics for the fixed states only in the first iteration:
int start = free_vars_start_;
int end = free_vars_end_;
if(iteration_ == 0)
{
start = 0;
end = num_vars_all_ - 1;
}
is_collision_free_ = true;
ros::WallDuration total_dur(0.0);
// for each point in the trajectory
for(int i = start; i <= end; ++i)
{
// Set Robot state from trajectory point...
collision_detection::CollisionRequest req;
collision_detection::CollisionResult res;
req.group_name = planning_group_;
setRobotStateFromPoint(group_trajectory_, i);
ros::WallTime grad = ros::WallTime::now();
hy_world_->getCollisionGradients(req,
res,
*hy_robot_->getCollisionRobotDistanceField().get(),
state_,
NULL,
gsr_);
total_dur += (ros::WallTime::now()-grad);
computeJointProperties(i);
state_is_in_collision_[i] = false;
//Keep vars in scope
{
size_t j = 0;
for(size_t g = 0; g < gsr_->gradients_.size(); g++)
{
collision_detection::GradientInfo& info = gsr_->gradients_[g];
for(size_t k = 0; k < info.sphere_locations.size(); k++)
{
collision_point_pos_eigen_[i][j][0] = info.sphere_locations[k].x();
collision_point_pos_eigen_[i][j][1] = info.sphere_locations[k].y();
collision_point_pos_eigen_[i][j][2] = info.sphere_locations[k].z();
collision_point_potential_[i][j] = getPotential(info.distances[k], info.sphere_radii[k], parameters_->getMinClearence());
collision_point_potential_gradient_[i][j][0] = info.gradients[k].x();
collision_point_potential_gradient_[i][j][1] = info.gradients[k].y();
collision_point_potential_gradient_[i][j][2] = info.gradients[k].z();
point_is_in_collision_[i][j] = (info.distances[k] - info.sphere_radii[k] < info.sphere_radii[k]);
if(point_is_in_collision_[i][j])
{
state_is_in_collision_[i] = true;
// if(is_collision_free_ == true) {
// ROS_INFO_STREAM("We know it's not collision free " << g);
// ROS_INFO_STREAM("Sphere location " << info.sphere_locations[k].x() << " " << info.sphere_locations[k].y() << " " << info.sphere_locations[k].z());
// ROS_INFO_STREAM("Gradient " << info.gradients[k].x() << " " << info.gradients[k].y() << " " << info.gradients[k].z() << " distance " << info.distances[k] << " radii " << info.sphere_radii[k]);
// ROS_INFO_STREAM("Radius " << info.sphere_radii[k] << " potential " << collision_point_potential_[i][j]);
// }
is_collision_free_ = false;
}
j++;
}
}
}
}
//ROS_INFO_STREAM("Total dur " << total_dur << " total checks " << end-start+1);
// now, get the vel and acc for each collision point (using finite differencing)
for(int i = free_vars_start_; i <= free_vars_end_; i++)
{
for(int j = 0; j < num_collision_points_; j++)
{
collision_point_vel_eigen_[i][j] = Eigen::Vector3d(0,0,0);
collision_point_acc_eigen_[i][j] = Eigen::Vector3d(0,0,0);
for(int k = -DIFF_RULE_LENGTH / 2; k <= DIFF_RULE_LENGTH / 2; k++)
{
collision_point_vel_eigen_[i][j] += (inv_time * DIFF_RULES[0][k + DIFF_RULE_LENGTH / 2]) * collision_point_pos_eigen_[i
+ k][j];
collision_point_acc_eigen_[i][j] += (inv_time_sq * DIFF_RULES[1][k + DIFF_RULE_LENGTH / 2]) * collision_point_pos_eigen_[i
+ k][j];
}
// get the norm of the velocity:
collision_point_vel_mag_[i][j] = collision_point_vel_eigen_[i][j].norm();
}
}
}
