void imProcessCalcRotateSize(int width, int height, int *new_width, int *new_height, double cos0, double sin0)
{
float xl, yl, xmin, xmax, ymin, ymax;
float wd2 = float(width)/2;
float hd2 = float(height)/2;
rotate_transf(wd2, hd2, 0, 0, &xl, &yl, cos0, sin0);
xmin = xl; ymin = yl;
xmax = xl; ymax = yl;
rotate_transf(wd2, hd2, width-1, height-1, &xl, &yl, cos0, sin0);
xmin = min_op(xmin, xl); ymin = min_op(ymin, yl);
xmax = max_op(xmax, xl); ymax = max_op(ymax, yl);
rotate_transf(wd2, hd2, 0, height-1, &xl, &yl, cos0, sin0);
xmin = min_op(xmin, xl); ymin = min_op(ymin, yl);
xmax = max_op(xmax, xl); ymax = max_op(ymax, yl);
rotate_transf(wd2, hd2, width-1, 0, &xl, &yl, cos0, sin0);
xmin = min_op(xmin, xl); ymin = min_op(ymin, yl);
xmax = max_op(xmax, xl); ymax = max_op(ymax, yl);
*new_width = (int)(xmax - xmin + 2.0);
*new_height = (int)(ymax - ymin + 2.0);
}
transf PositionStateApproach::getCoreTran() const
{
double dist = readVariable("dist");
double rx = readVariable("wrist 1");
double ry = readVariable("wrist 2");
transf handTran = transf(Quaternion::IDENTITY, vec3(0,0,dist));
handTran = handTran * rotate_transf(rx, vec3(1,0,0) ) * rotate_transf( ry, vec3(0,1,0) );
return mHand->getApproachTran().inverse() * handTran * mHand->getApproachTran();
}
/*!
Sets a new theta value for revolute joints and recomputes the current value
of the transform.
*/
void DHTransform::setTheta(double q)
{
theta = q;
tr1 = rotate_transf(theta,vec3(0,0,1));
tran = tr4TimesTr3 * tr2 * tr1;
}
void
CGDBGraspProcessor::processGrasps(std::vector<db_planner::Grasp*> *grasps)
{
std::vector<db_planner::Grasp*>::iterator it;
for (it=grasps->begin(); it!=grasps->end(); it++) {
if (mMaxGrasps >=0 && mProcessedGrasps >= mMaxGrasps) break;
//silently assume that the grasps are actually GraspitDBGrasp
static_cast<GraspitDBGrasp*>(*it)->getFinalGraspPlanningState()->execute();
/*
//hack for a few grasps which seem not to have f-c
if ((*it)->EpsilonQuality() == 0.00685985 ||
(*it)->EpsilonQuality() == 0.00647612 ) {
DBGA("Skipping grasp with epsilon quality: " << (*it)->EpsilonQuality());
continue;
}
*/
mHand->getWorld()->findAllContacts();
mHand->getWorld()->updateGrasps();
int numContacts = mHand->getGrasp()->getNumContacts();
DBGA("Processing grasp number " << mProcessedGrasps << "; " << numContacts << " contacts.");
mProcessor->processGrasp();
//hack for the mcgrip optimizer; also transpose the grasp
bool transposeGrasp = true;
if (transposeGrasp) {
//interchange the dofs of the two chain
double dofs[6];
mHand->getDOFVals(dofs);
for (int j=0; j<3; j++) {
std::swap(dofs[j], dofs[3+j]);
}
mHand->forceDOFVals(dofs);
//rotate the hand
transf handTran = rotate_transf(M_PI, vec3::Z);
transf newTran = mHand->getApproachTran().inverse() * handTran *
mHand->getApproachTran() * mHand->getTran();
mHand->setTran(newTran);
if (!mHand->getWorld()->noCollision()) {
DBGA("Transposed grasp in COLLISION!!!");
assert(0);
}
mHand->getWorld()->findAllContacts();
mHand->getWorld()->updateGrasps();
if (mHand->getGrasp()->getNumContacts() != numContacts) {
DBGA("Transposed grasp has " << mHand->getGrasp()->getNumContacts() << " contacts.");
continue;
assert(0);
}
DBGA("Processing grasp transpose");
mProcessor->processGrasp();
}
mProcessedGrasps ++;
}
}
/*! Initializes the DHTransform with the 4 DH parameters. */
DHTransform::DHTransform(double thval,double dval,double aval,double alval) :
theta(thval),d(dval),a(aval),alpha(alval)
{
transf tr3,tr4;
dtrans[0] = 0.0;
dtrans[1] = 0.0;
dtrans[2] = d;
atrans[0] = a;
atrans[1] = 0.0;
atrans[2] = 0.0;
tr1 = rotate_transf(theta,vec3(0,0,1));
tr2 = translate_transf(dtrans);
tr3 = translate_transf(atrans);
tr4 = rotate_transf(alpha,vec3(1,0,0));
tr4TimesTr3 = tr4 * tr3;
tran = tr4TimesTr3 * tr2 * tr1;
}
/*!
Sets a new theta value for the linear offset along x and recomputes the current value
of the transform.
*/
void DHTransform::setA(double q)
{
a = q;
atrans[0] = a;
transf tr3,tr4;
tr3 = translate_transf(atrans);
tr4 = rotate_transf(alpha,vec3(1,0,0));
tr4TimesTr3 = tr4 * tr3;
tran = tr4TimesTr3 * tr2 * tr1;
}
transf PositionStateAA::getCoreTran() const
{
double tx = readVariable("Tx");
double ty = readVariable("Ty");
double tz = readVariable("Tz");
double theta = readVariable("theta");
double phi = readVariable("phi");
double alpha = readVariable("alpha");
transf coreTran = rotate_transf(alpha, vec3( sin(theta)*cos(phi) , sin(theta)*sin(phi) , cos(theta) )) *
translate_transf(vec3(tx,ty,tz));
//transform now returned relative to hand approach transform
return mHand->getApproachTran().inverse() * coreTran;
}
void Leaf::computeBboxOO()
{
//compute the covariance matrix
double covMat[3][3], v[3][3];
areaWeightedCovarianceMatrix(covMat);
DBGP("Cov mat:"); DBGST(print(covMat));
//perform singular value decomposition
Jacobi(covMat, v);
DBGP("eigenvalues:"); DBGST(print(covMat));
DBGP("eigenVectors:"); DBGST(print(v));
int first = 0, second = 1, third = 2;
if (covMat[1][1] > covMat[0][0]) {
std::swap(first, second);
}
if (covMat[2][2] > covMat[first][first]) {
std::swap(first, third);
}
if (covMat[third][third] > covMat[second][second]) {
std::swap(second, third);
}
DBGP("Eigenvalues: " << covMat[first][first] << " " << covMat[second][second]
<< " " << covMat[third][third]);
//set up rotation matrix
vec3 xAxis(v[0][first], v[1][first], v[2][first]);
vec3 yAxis(v[0][second], v[1][second], v[2][second]);
vec3 zAxis = normalise(xAxis) * normalise(yAxis);
yAxis = zAxis * normalise(xAxis);
xAxis = yAxis * zAxis;
mat3 R(xAxis, yAxis, zAxis);
DBGP("Matrix: " << R);
//compute bbox extents
vec3 halfSize, center;
fitBox(R, center, halfSize);
//rotate box so that x axis always points in the direction of largest extent
first = 0;
if (halfSize.y() > halfSize.x()) first = 1;
if (halfSize.z() > halfSize[first]) first = 2;
transf rotate = transf::IDENTITY;
if (first == 1) {
// y has the largest extent, rotate around z
rotate = rotate_transf(M_PI/2.0, vec3(0,0,1));
} else if (first == 2) {
// z has the largest extent, rotate around y
rotate = rotate_transf(M_PI/2.0, vec3(0,1,0));
}
halfSize = halfSize * rotate;
for (int i=0; i<3; i++) {
if (halfSize[i] < 0) halfSize[i] = -halfSize[i];
}
mat3 RR;
rotate.rotation().ToRotationMatrix(RR);
R = RR * R;
mBbox.halfSize = halfSize;
mBbox.setTran( transf(R, center ) );
}
BodyState
Controller::getDesiredState(quater trunk, const BodyState& prevState)
{
// 요기
BodyState bs;
float dT = sysState->TIME_STEP;
bool isLeft;
#ifdef OLD
float offsetX = -20;
float offsetY = -5;
float offsetZ = 3;//10;
#else
float offsetX = -25;
float offsetY = 0;
float offsetZ = 0;//10;
#endif
quater offset;
for(int i=0;i<2;i++)
{
if(i==0) isLeft = true;
else isLeft = false;
if(isLeft)
{
offset = rotate_transf(DegToRad(offsetX), vector3(1,0,0)).getRotation();
offset = offset * rotate_transf(DegToRad(offsetY), vector3(0,1,0)).getRotation();
offset = offset * rotate_transf(DegToRad(offsetZ), vector3(0,0,1)).getRotation();
}
else
{
offset = rotate_transf(DegToRad(offsetX), vector3(1,0,0)).getRotation();
offset = offset * rotate_transf(DegToRad(-offsetY), vector3(0,1,0)).getRotation();
offset = offset * rotate_transf(DegToRad(-offsetZ), vector3(0,0,1)).getRotation();
}
bs.shoulder[i] = trunk * offset * realWingbeat.getShoulder(currentFrame, currentWeight, isLeft);
bs.elbowBend[i] = realWingbeat.getElbowBend(currentFrame, currentWeight, isLeft);
bs.elbowTwist[i] = realWingbeat.getElbowTwist(currentFrame, currentWeight, isLeft);
bs.wristBend[i] = realWingbeat.getWrist(currentFrame, currentWeight, isLeft);
bs.dShoulder[i] = GET_ANGULAR_VELOCITY_QUATER( bs.shoulder[i], prevState.shoulder[i], dT );
bs.dElbowBend[i] = (bs.elbowBend[i] - prevState.elbowBend[i]) / dT;
bs.dElbowTwist[i] = (bs.elbowTwist[i] - prevState.elbowTwist[i]) / dT;
bs.dWristBend[i] = (bs.wristBend[i] - prevState.wristBend[i]) / dT;
bs.ddShoulder[i] = (bs.dShoulder[i] - prevState.dShoulder[i]) / dT;
bs.ddElbowBend[i] = (bs.dElbowBend[i] - prevState.dElbowBend[i]) / dT;
bs.ddElbowTwist[i] = (bs.dElbowTwist[i] - prevState.dElbowTwist[i]) / dT;
bs.ddWristBend[i] = (bs.dWristBend[i] - prevState.dWristBend[i]) / dT;
// print
//WRITE_ARRAY4(cout, bs.shoulder[i]);
//cout << "angle " << RadToAng(bs.elbowBend[i]) << "," << RadToAng(bs.elbowTwist[i]) << "," << RadToAng(bs.wristBend[i]) << endl;
//WRITE_ARRAY3(cout, bs.dShoulder[i]);
//cout << "angle vel " << RadToAng(bs.dElbowBend[i]) << "," << RadToAng(bs.dElbowTwist[i]) << "," << RadToAng(bs.dWristBend[i]) << endl;
}
return bs;
}
