void Solver::DiagramQd(){
double time0, time1;
time0 = omp_get_wtime();
#pragma omp parallel
{
int id = omp_get_thread_num();
int threads = omp_get_num_threads();
for (int n=id; n<Nkphh; n+=threads) blockskphh[n]->SetUpMatrices_Qd(t0);
}
time1 = omp_get_wtime(); //cout << "Inside Qd. SetUpMatrices needs: " << time1-time0 << " seconds" << endl;
time0 = omp_get_wtime();
for (int n=0; n<Nkphh; n++){
mat Qd = 0.5*blockskphh[n]->T * blockskphh[n]->V * blockskphh[n]->T2 / blockskphh[n]->epsilon;
for (int k1=0; k1<blockskphh[n]->Nk1; k1++){
for (int k2=0; k2<blockskphh[n]->Nk3; k2++){
int a=blockskphh[n]->K1(k1,0); int b=blockskphh[n]->K3(k2,0);
int i=blockskphh[n]->K3(k2,1); int j=blockskphh[n]->K3(k2,2);
t( Index(a,b,i,j)) -= Qd( k2,k1 );
t( Index(b,a,i,j)) += Qd( k2,k1 );
//StoreT(Index(a,b,i,j), -Qd(k2,k1));
//StoreT(Index(b,a,i,j), Qd(k2,k1));
}
}
}
time1 = omp_get_wtime(); //cout << "Inside Qd. Calculate matrices needs: " << time1-time0 << " seconds" << endl;
}
void Camera::lookAt(const Point<double> &target)
{
Vd dir = ~Vd(target-origin);
double len = sqrt(dir.x * dir.x + dir.y * dir.y);
double pitch = atan2(dir.z, len);
double pan = atan2(dir.x, dir.y);
objective = Qd(Rd(-pitch, Vd(1, 0, 0))) * Qd(Rd(pan, Vd(0, 0, 1)));
}
void Controller::lookZ(double speed)
{
speed *= LookSpeed;
speed *= FRATE;
Qd buffer = camAngle;//Used to rollback if out of bounds
Vector<double> mystery = ~(camAngle * Vector<double>(0,1,0));
double mysteryYaw = atan2(mystery.x, mystery.y);
camAngle = Qd(Rd(speed, Vd(cos(mysteryYaw),-sin(mysteryYaw),0))) * camAngle;
if((camAngle*Vector<double>(0,1,0)).z > 0.99 || (camAngle*Vector<double>(0,1,0)).z < -0.99){
camAngle = buffer;
return;
}
Vector<double> vec = ~(camAngle * Vector<double>(0,1,0));
if (firstPerson == true)
{
camera.origin = player->origin + Pd(0,0,2.5) + vec;
camera.lookAt(camera.origin + (vec * 5.0));
}
else
{
camera.origin = target - (vec * zoom);
camera.lookAt(target);
}
}
/*---------------------------------------------------------------------------*/
bool QfsIsWritable (QFileInfo Qi) /* no file => check if dir is writable */
{
if ( Qi .exists()) return Qi.isWritable();
else if (!Qi.dir().exists()) return false;
else {
QFileInfo Qd(Qi.path()); return Qd.isWritable();
//+
// fprintf(stderr, "Dir(%s)%s[%x]\n", Qd.fileName().cStr(),
//- Qd.isWritable() ? "writable" : "read-only", (int)Qd.permissions());
} }
Controller::Controller(Camera &C, ObjectHandle P) : camera(C), player(P)
{
firstPerson = false;
lastView = false;
target = player->origin + Pd(-.5,-.5,2);
Vector<double> vec = camAngle * Vector<double>(0,1,0);
zoom = 10.0;
camera.origin = target - (~vec * zoom);
camera.lookAt(target);
camAngle = Qd();
blocked = false;
}
Cuboid::Cuboid(Pd origin, double S)
: BoundedObject(origin, Qd(), BoundingBox(), Assets::Test),
u(Vd(S,0,0)),
v(Vd(0,S,0)),
w(Vd(0,0,S))
{
bb.lbl = origin ;
bb.rbl = origin +u ;
bb.ltl = origin +v ;
bb.rtl = origin +u +v ;
bb.lbh = origin +w;
bb.rbh = origin +u +w;
bb.lth = origin +v +w;
bb.rth = origin +u +v +w;
}
Droppable::Droppable(Pd _origin, Resource _worth, long _dropped, long _ttl)
: BoundedObject(_origin, Qd(), BoundingBox(Pd(-0.105, -0.5, 0.0), Pd(0.105, 0.5, 1.0)))
{
model.coin = ModelObjectContainer();
model.coin->children.insert(Assets::Model::CoinObj);
children.insert(model.coin);
model.coin->material = Assets::Model::CoinTex;
worth = _worth;
dropped = _dropped;
ttl = _ttl;
done = false;
long pid = (Game::game.player ? Game::game.player->id : 0);
id = topId++ | (pid << 16);
}
void Controller::lookX(double speed)
{
speed *= -LookSpeed;
speed *= FRATE;
camAngle = Qd(Rd(speed, Vd(0,0,1))) * camAngle;
Vector<double> vec = ~(camAngle * Vector<double>(0,1,0));
if (firstPerson == true)
{
camera.origin = player->origin + Pd(0,0,2.5) + vec;
camera.lookAt(camera.origin + (vec * 5.0));
}
else
{
camera.origin = target - (vec * zoom);
camera.lookAt(target);
}
}
World::World(double _width, double _height)
: BoundedObject(Pd(), Qd(),
BoundingBox(Pd(-_width/2, -_height/2, 0), Pd(_width,0,0), Pd(0,_height,0), Pd(_width,_height,0), Pd(), Pd(), Pd(), Pd(_width/2, _height/2, HIGH)),
Assets::WorldMaterial)
{
ObjectHandle tHandle;
tHandle = Terrain(_width, _height);
terrain = TO(Terrain, tHandle);
children.insert(tHandle);
ObjectHandle hudHandle;
hudHandle = HUD(640, 480);
hud = TO(HUD, hudHandle);
children.insert(hudHandle);
children.insert(Sky((int) _width, (int) _height));
width = _width;
height = _height;
}
bool OrientVectorToVectorTask::getCommand(ControlModel & model, TaskCommand & command)
{
// Get the latest joint state information
Vector Q(model.getNumDOFs());
Vector Qd(model.getNumDOFs());
model.getLatestFullState(Q, Qd);
// Get orientation of the local body vector in the world frame
Rbody = RigidBodyDynamics::CalcBodyWorldOrientation(model.rbdlModel(), Q, bodyId_, false); // TODO: Use Latest State
if (tare)
{
CONTROLIT_INFO_RT << "Taring the goal orientation!";
goalVector_ = Rbody.transpose() * bodyFrameVector_; // assumes goalVector_ is in world coordinate frame
tare = 0;
}
// Just in case, normalize the current and goal vectors
bodyFrameVector_.normalize();
goalVector_.normalize();
// Check if latched status has been updated
updateLatch(&model);
getJacobian(Jtloc);
// Compute the goal vector in the frameName_ coordinate frame
// goalHeading = goalVector_;
paramGoalHeading->set(goalVector_); // sets member variable "goalHeading"
if(frameId_ != -1) // The goal vector is in robot frame, either latched or not
{
if(isLatched)
Rframe = latchedRotation;
else
Rframe = RigidBodyDynamics::CalcBodyWorldOrientation(model.rbdlModel(), Q, frameId_, false); // TODO: Use Latest State
goalHeading = Rframe.transpose() * goalVector_;
}
// Get the heading in the frameName_ coordinate frame
actualHeading = Rbody.transpose() * bodyFrameVector_;
paramActualHeading->set(actualHeading);
// Compute the error (goal heading - current heading)
/*if (goalHeading.cols() != actualHeading.cols() || goalHeading.rows() != actualHeading.rows())
{
CONTROLIT_ERROR << "Matrix size error:\n"
<< " - goalHeading =\n" << goalHeading << "\n"
<< " - goalVector =\n" << goalVector_ << "\n"
<< " - frameId_ = " << frameId_ << "\n"
<< " - RFrame =\n" << Rframe << "\n"
<< " - actualHeading=\n" << actualHeading;
}*/
e0 = goalHeading - actualHeading;
// Compute the angle between the goal heading and current heading.
// The equation is cos^-1(goalHeading dot actualHeading / (|goalHeading| * |actualHeading|)).
double error = std::acos(goalHeading.dot(actualHeading) / (goalHeading.norm() * actualHeading.norm()))
* 180 / 3.141592653589793238463;
paramErrorAngle->set(error);
// Set the command type
command.type = commandType_;
// Compute the command
// The goal velocity is zero, thus the velocity error is -Jtloc * Qd.
controller->computeCommand(e0, -Jtloc * Qd, command.command, this);
Vector base_vector;
base_vector.setZero(3);
Vector tran_BodyToBase = RigidBodyDynamics::CalcBodyToBaseCoordinates(model.rbdlModel(), Q, bodyId_, base_vector, false);
// If we are able to grab the lock on visualizationPublisher,
// publish the visualization markers.
if (visualizationPublisher.trylock())
{
// Create a marker showing the goal vector
visualizationPublisher.msg_.markers[0].header.stamp = ros::Time::now();
visualizationPublisher.msg_.markers[0].points[0].x = tran_BodyToBase[0];
visualizationPublisher.msg_.markers[0].points[0].y = tran_BodyToBase[1];
visualizationPublisher.msg_.markers[0].points[0].z =tran_BodyToBase[2];
visualizationPublisher.msg_.markers[0].points[1].x = tran_BodyToBase[0] + goalHeading(0);
visualizationPublisher.msg_.markers[0].points[1].y = tran_BodyToBase[1] + goalHeading(1);
visualizationPublisher.msg_.markers[0].points[1].z = tran_BodyToBase[2] + goalHeading(2);
// Create a marker showing the current heading
visualizationPublisher.msg_.markers[1].header.stamp = ros::Time::now();
visualizationPublisher.msg_.markers[1].points[1].x = bodyFrameVector_(0);
visualizationPublisher.msg_.markers[1].points[1].y = bodyFrameVector_(1);
visualizationPublisher.msg_.markers[1].points[1].z = bodyFrameVector_(2);
// Publish the MarkerArray
visualizationPublisher.unlockAndPublish();
}
return true;
}
Star::Star(Pd P)
: Object(P, Qd(), Assets::Star)
{
big = false;
}
