Socket_one::Socket_one(std::string name, const tf::Vector3& origin, const tf::Vector3 &rpy,float scale){
//const std::string& name, const geo::fCVec3& C, const geo::fMat33& R, float r
geo::fMat33 R;
R.eye();
disk_radius = 0.02 * scale;
hole_radius = 0.002 * scale;
std::array<geo::Disk,3> holes;
geo::fCVec3 position;
position.zeros();
geo::Disk plate("plate",position,R,disk_radius);
position.zeros();
position(0) = -0.01 * scale;
holes[0] = geo::Disk("hole_left",position,R,hole_radius);
position.zeros();
position(0) = 0.01 * scale;
holes[1] = geo::Disk("hole_right",position,R,hole_radius);
position.zeros();
position(1) = 0.005 * scale;
holes[2] = geo::Disk("hole_up",position,R,hole_radius);
wsocket = wobj::WSocket(name,plate,holes);
geo::fCVec3 dim = {{0.07,0.07,0.05}};
dim = dim * scale;
geo::fCVec3 orientation = {{0,0,0}};
position.zeros();
position(2) = -0.005/2 - (0.05 - 0.005)/2;
wbox = wobj::WBox("box_socket_one",dim,position,orientation);
// Transformation
orientation(0) = rpy.x();
orientation(1) = rpy.y();
orientation(2) = rpy.z();
geo::fCVec3 T = {{origin.x(),origin.y(),origin.z()}};
wsocket.transform(T,orientation);
wbox.transform(T,orientation);
}
BaseIF* makePlate(const Real& height,
const Real& thick,
const Real& radius,
const int& doHoles,
const Real& holeRadius,
const Real& holeSpace)
{
RealVect zero(D_DECL(0.0,0.0,0.0));
RealVect xAxis(D_DECL(1.0,0.0,0.0));
bool inside = true;
// Create the plate without holes
Vector<BaseIF*> pieces;
RealVect normal1(D_DECL(1.0,0.0,0.0));
RealVect point1(D_DECL(height,0.0,0.0));
PlaneIF plane1(normal1,point1,inside);
pieces.push_back(&plane1);
RealVect normal2(D_DECL(-1.0,0.0,0.0));
RealVect point2(D_DECL(height+thick,0.0,0.0));
PlaneIF plane2(normal2,point2,inside);
pieces.push_back(&plane2);
TiltedCylinderIF middle(radius,xAxis,zero,inside);
pieces.push_back(&middle);
IntersectionIF plate(pieces);
// Make the drills
Vector<BaseIF*> drillBits;
// Compute how many drills are needed in each direciton - 2*num+1 -
// conservatively
int num = (int)((radius - holeRadius) / holeSpace + 1.0);
if (doHoles != 0)
{
for (int i = -num; i <= num; i++)
{
for (int j = -num; j <= num; j++)
{
RealVect center(D_DECL(0.0,i*holeSpace,j*holeSpace));
TiltedCylinderIF* drill = new TiltedCylinderIF(holeRadius,xAxis,center,inside);
drillBits.push_back(drill);
}
}
}
UnionIF drills(drillBits);
ComplementIF notDrills(drills,true);
// Drill the plate
IntersectionIF* holyPlate = new IntersectionIF(plate,notDrills);
return holyPlate;
}
