void TrajectoryPlannerTest::footprintObstacles(){
//place an obstacle
map_(4, 6).occ_state = 1;
wa->synchronize();
EXPECT_EQ(wa->getCost(4,6), costmap_2d::LETHAL_OBSTACLE);
Trajectory traj(0, 0, 0, 30);
//tc->generateTrajectory(4.5, 4.5, M_PI_2, 0, 0, 0, 4, 0, 0, 4, 0, 0, DBL_MAX, traj, 2, 30);
tc.generateTrajectory(4.5, 4.5, M_PI_2, 0, 0, 0, 4, 0, 0, 4, 0, 0, DBL_MAX, traj);
//we expect this path to hit the obstacle
EXPECT_FLOAT_EQ(traj.cost_, -1.0);
//place a wall next to the footprint of the robot
map_(7, 1).occ_state = 1;
map_(7, 3).occ_state = 1;
map_(7, 4).occ_state = 1;
map_(7, 5).occ_state = 1;
map_(7, 6).occ_state = 1;
map_(7, 7).occ_state = 1;
wa->synchronize();
//try to rotate into it
//tc->generateTrajectory(4.5, 4.5, M_PI_2, 0, 0, 0, 0, 0, M_PI_2, 0, 0, M_PI_4, 100, traj, 2, 30);
tc.generateTrajectory(4.5, 4.5, M_PI_2, 0, 0, 0, 0, 0, M_PI_2, 0, 0, M_PI_4, 100, traj);
//we expect this path to hit the obstacle
EXPECT_FLOAT_EQ(traj.cost_, -1.0);
}
int main() {
chemfiles::Trajectory traj("filename.nc");
chemfiles::Frame frame;
std::vector<double> distances;
// Accumulate the distances to the origin of the 10th atom throughtout the
// trajectory
while (!traj.done()) {
traj >> frame;
// Position of the 10th atom
auto pos = frame.positions()[9];
double distance = sqrt(pos[0]*pos[0] + pos[1]*pos[1] + pos[2]*pos[2]);
distances.push_back(distance);
}
double mean = std::accumulate(std::begin(distances), std::end(distances), 0.0) / distances.size();
double rmsd = 0.0;
for (auto dist : distances) {
rmsd += (mean - dist)*(mean - dist);
}
rmsd /= distances.size();
rmsd = sqrt(rmsd);
std::cout << "Root-mean square displacement is: " << rmsd << std::endl;
return 0;
}
int main()
{
// Primary Operators
AnnihilationOperator A1(0); // 1st freedom
NumberOperator N1(0);
IdentityOperator Id1(0);
AnnihilationOperator A2(1); // 2nd freedom
NumberOperator N2(1);
IdentityOperator Id2(1);
SigmaPlus Sp(2); // 3rd freedom
IdentityOperator Id3(2);
Operator Sm = Sp.hc(); // Hermitian conjugate
Operator Ac1 = A1.hc();
Operator Ac2 = A2.hc();
// Hamiltonian
double E = 20.0;
double chi = 0.4;
double omega = -0.7;
double eta = 0.001;
Complex I(0.0,1.0);
Operator H = (E*I)*(Ac1-A1)
+ (0.5*chi*I)*(Ac1*Ac1*A2 - A1*A1*Ac2)
+ omega*Sp*Sm + (eta*I)*(A2*Sp-Ac2*Sm);
// Lindblad operators
double gamma1 = 1.0;
double gamma2 = 1.0;
double kappa = 0.1;
const int nL = 3;
Operator L[nL]={sqrt(2*gamma1)*A1,sqrt(2*gamma2)*A2,sqrt(2*kappa)*Sm};
// Initial state
State phi1(50,FIELD); // see paper Section 4.2
State phi2(50,FIELD);
State phi3(2,SPIN);
State stateList[3] = {phi1,phi2,phi3};
State psiIni(3,stateList);
// Trajectory
double dt = 0.01; // basic time step
int numdts = 100; // time interval between outputs = numdts*dt
int numsteps = 5; // total integration time = numsteps*numdts*dt
int nOfMovingFreedoms = 2;
double epsilon = 0.01; // cutoff probability
int nPad = 2; // pad size
//ACG gen(38388389); // random number generator with seed
//ComplexNormal rndm(&gen); // Complex Gaussian random numbers
ComplexNormalTest rndm(899101); // Simple portable random number generator
AdaptiveStep stepper(psiIni, H, nL, L); // see paper Section 5
// Output
const int nOfOut = 3;
Operator outlist[nOfOut]={ Sp*A2*Sm*Sp, Sm*Sp*A2*Sm, A2 };
char *flist[nOfOut]={"X1.out","X2.out","A2.out"};
int pipe[] = {1,5,9,11}; // controls standard output (see `onespin.cc')
// Simulate one trajectory (for several trajectories see `onespin.cc')
Trajectory traj(psiIni, dt, stepper, &rndm); // see paper Section 5
traj.plotExp( nOfOut, outlist, flist, pipe, numdts, numsteps,
nOfMovingFreedoms, epsilon, nPad );
}
void LegMotion::InitWalk(Eigen::Vector2f destination, Eigen::Vector2f startingFeetAngles, Eigen::Vector2f destinationFeetAngles)
{
m_bIsUsingAlgorithm = true;
std::unique_ptr<Trajectory> traj( new Trajectory(m_vRightFootPosOffset, m_vRightFootAngleOffset,
m_vLeftFootPosOffset, m_vLeftFootAngleOffset, m_vRightPelvisPosOffset, m_vRightPelvisAngleOffset,
m_vLeftPelvisPosOffset, m_vLeftPelvisAngleOffset, m_pelvisPermanentPitch, m_stepLength) );
m_trajectoryMatrix = traj->GenerateWalk(Eigen::Vector2f(0, 0), destination,
destinationFeetAngles, startingFeetAngles, m_pelvisTrajectoryType, m_stepTime, m_stepHeight);
}
void LegMotion::InitKick(float ratioKickSpeed, float kickTime)
{
m_bIsUsingAlgorithm = true;
std::unique_ptr<Trajectory> traj( new Trajectory(m_vRightFootPosOffset, m_vRightFootAngleOffset,
m_vLeftFootPosOffset, m_vLeftFootAngleOffset, m_vRightPelvisPosOffset, m_vRightPelvisAngleOffset,
m_vLeftPelvisPosOffset, m_vLeftPelvisAngleOffset, m_pelvisPermanentPitch, m_stepLength) );
m_trajectoryMatrix = traj->GenerateKick(ratioKickSpeed, kickTime, m_PelvisKickOffsetR, m_KickBackOffsetR, m_KickForwardOffsetR);
}
TEST_F(TrajectoryLibraryTest, MinimumAltitude) {
Trajectory traj("trajtest/simple/two-point-00000", true);
EXPECT_EQ_ARM(traj.GetMinimumAltitude(), 0);
Trajectory traj2("trajtest/full/unit-testing-super-aggressive-dive-open-loop-00009", true);
EXPECT_EQ_ARM(traj2.GetMinimumAltitude(), -7.9945);
Trajectory traj3("trajtest/full/unit-testing-knife-edge-open-loop-00010", true);
EXPECT_EQ_ARM(traj3.GetMinimumAltitude(), -1.2897);
}
/**
* Tests point transformation with a simple two-point trajectory
*/
TEST_F(TrajectoryLibraryTest, GetTransformedPoint) {
Trajectory traj("trajtest/simple/two-point-00000", true);
BotTrans trans;
bot_trans_set_identity(&trans);
trans.trans_vec[0] = 1;
double point[3] = {1, 0, 0};
double output[3];
traj.GetXyzYawTransformedPoint(0, trans, output);
//std::cout << "point = (" << point[0] << ", " << point[1] << ", " << point[2] << ")" << std::endl;
//std::cout << "output = (" << output[0] << ", " << output[1] << ", " << output[2] << ")" << std::endl;
for (int i = 0; i < 3; i++) {
EXPECT_NEAR(point[i], output[i], TOLERANCE);
}
trans.trans_vec[0] = 0;
traj.GetXyzYawTransformedPoint(1, trans, output);
for (int i = 0; i < 3; i++) {
EXPECT_NEAR(point[i], output[i], TOLERANCE);
}
// transform with rotation
trans.rot_quat[0] = 0.707106781186547;
trans.rot_quat[1] = 0;
trans.rot_quat[2] = 0;
trans.rot_quat[3] = 0.707106781186547;
traj.GetXyzYawTransformedPoint(1, trans, output);
point[0] = 0;
point[1] = 1;
point[2] = 0;
//std::cout << "point = (" << point[0] << ", " << point[1] << ", " << point[2] << ")" << std::endl;
//std::cout << "output = (" << output[0] << ", " << output[1] << ", " << output[2] << ")" << std::endl;
for (int i = 0; i < 3; i++) {
EXPECT_NEAR(point[i], output[i], TOLERANCE);
}
}
TEST_F(TrajectoryLibraryTest, CheckBounds) {
Trajectory traj("trajtest/simple/two-point-00000", true);
BotTrans trans;
bot_trans_set_identity(&trans);
double output[3];
traj.GetXyzYawTransformedPoint(0, trans, output);
EXPECT_EQ_ARM(output[0], 0);
traj.GetXyzYawTransformedPoint(0.01, trans, output);
EXPECT_EQ_ARM(output[0], 1);
traj.GetXyzYawTransformedPoint(10, trans, output);
EXPECT_EQ_ARM(output[0], 1);
}
/**
* Loads a trajectory with two points and does basic manipulation of it.
*/
TEST_F(TrajectoryLibraryTest, LoadTrajectory) {
// load a test trajectory
Trajectory traj("trajtest/simple/two-point-00000", true);
// ensure that we can access the right bits
EXPECT_EQ_ARM(traj.GetDimension(), 12);
EXPECT_EQ_ARM(traj.GetUDimension(), 3);
Eigen::VectorXd output = traj.GetState(0);
Eigen::VectorXd origin(12);
origin << 0, 0, 0, 0, 0, 0, 10, 0, 0, 0, 0, 0;
EXPECT_APPROX_MAT( origin, output, TOLERANCE);
Eigen::VectorXd point(12);
point << 1, 0, 0, 0, 0, 0, 10, 0, 0, 0, 0, 0;
output = traj.GetState(0.01);
EXPECT_APPROX_MAT(output, point, TOLERANCE);
Eigen::VectorXd upoint(3);
upoint << 0, 0, 0;
EXPECT_APPROX_MAT(upoint, traj.GetUCommand(0), TOLERANCE);
Eigen::VectorXd upoint2(3);
upoint2 << 1, 2, 3;
EXPECT_APPROX_MAT(upoint2, traj.GetUCommand(0.01), TOLERANCE);
EXPECT_EQ_ARM(traj.GetTimeAtIndex(0), 0);
EXPECT_EQ_ARM(traj.GetTimeAtIndex(1), 0.01);
EXPECT_EQ_ARM(traj.GetNumberOfPoints(), 2);
}
std::vector<CvPoint> CaptureManager::GetTrajectory(int c)
{
std::vector<CvPoint> traj(frameCount);
for (int i=0; i<frameCount; i++)
{
if (Access(i,0,false, true)->contourArray.size() <= c)
traj[i]=cvPoint(-1,-1);
else
{
CvMoments m;
double m00,m10,m01;
cvMoments(Access(i,0,false, true)->contourArray[c], &m);
m00 = cvGetSpatialMoment(&m,0,0);
m01 = cvGetSpatialMoment(&m,0,1);
m10 = cvGetSpatialMoment(&m,1,0);
traj[i] = cvPoint(cvRound(m.m10/m.m00), cvRound(m.m01/m.m00));
}
}
return traj;
}
Mat CascadeDSController::IntegrateTrajectory(realtype dt, realtype speed_threshold, realtype t_max)
{
int n_max = int(t_max/dt);
Mat traj(dim_,n_max);
Vec rpos = filt_pos_-ds_origin_;
Vec rvel = rpos;
rvel.setZero();
int n = 0;
while(n<n_max){
traj.col(n)=rpos+ds_origin_;
rvel = task_dynamics_(rpos);
if(rvel.norm()<speed_threshold){
traj.resize(dim_,n+1);
break;
}
rpos += rvel*dt;
n++;
}
return traj;
}
int main() {
chemfiles::Trajectory traj("filename.xyz");
chemfiles::Frame frame;
traj >> frame;
auto positions = frame.positions();
std::vector<size_t> indexes;
for (size_t i=0; i<frame.natoms(); i++) {
if (positions[i][0] < 5) {
indexes.push_back(i);
}
}
std::cout << "Atoms with x < 5: " << std::endl;
for (auto i : indexes) {
std::cout << " - " << i << std::endl;
}
return 0;
}
TEST_F(TrajectoryLibraryTest, TestTiRollout) {
Trajectory traj("trajtest/ti/TI-test-TI-straight-pd-no-yaw-00000", true);
Eigen::VectorXd expected(12);
expected << 0,0,0,0,-0.19141,0,12.046,0,-2.3342,0,0,0;
Eigen::VectorXd output = traj.GetState(0);
EXPECT_APPROX_MAT(expected, output, TOLERANCE);
Eigen::VectorXd expected2(12);
expected2 << 26.135,0,9.2492e-09,0,-0.19141,0,12.046,0,-2.3342,0,7.8801e-13,0;
output = traj.GetState(2.13);
EXPECT_APPROX_MAT( expected2, output, TOLERANCE);
}
TEST_F(TrajectoryLibraryTest, RemainderTrajectoryTi) {
StereoOctomap octomap(bot_frames_);
double altitude = 30;
BotTrans trans;
bot_trans_set_identity(&trans);
trans.trans_vec[2] = altitude;
Trajectory traj("trajtest/ti/TI-test-TI-straight-pd-no-yaw-00000", true);
double dist = traj.ClosestObstacleInRemainderOfTrajectory(octomap, trans, 0, 0);
EXPECT_EQ_ARM(dist, -1);
double point[3] = { 1.5, -0.5, 3 };
AddPointToOctree(&octomap, point, altitude);
dist = traj.ClosestObstacleInRemainderOfTrajectory(octomap, trans, 0, 0);
EXPECT_NEAR(dist, 3.041506, TOLERANCE2); // from matlab
dist = traj.ClosestObstacleInRemainderOfTrajectory(octomap, trans, 1.21, 0);
EXPECT_NEAR(dist, 13.6886, TOLERANCE2);
}
int NFFT2DGadget::process(GadgetContainerMessage< ISMRMRD::AcquisitionHeader > *m1, // header
GadgetContainerMessage< hoNDArray< std::complex<float> > > *m2, // data
GadgetContainerMessage< hoNDArray<float> > *m3 ) // traj/dcw
{
// Throw away any noise samples if they have been allowed to pass this far down the chain...
//
bool is_noise = m1->getObjectPtr()->isFlagSet(ISMRMRD::ISMRMRD_ACQ_IS_NOISE_MEASUREMENT);
if (is_noise) {
m1->release();
return GADGET_OK;
}
// First pass initialization
//
if (frame_readout_queue_->message_count() == 0 ) {
samples_per_readout_ = m1->getObjectPtr()->number_of_samples;
num_coils_ = m1->getObjectPtr()->active_channels;
dimensions_.push_back(m1->getObjectPtr()->active_channels);
dimensions_.push_back(repetitions_);
num_trajectory_dims_ = m3->getObjectPtr()->get_size(0); // 2 for trajectories only, 3 for both trajectories + dcw
}
int samples = m1->getObjectPtr()->number_of_samples;
int readout = m1->getObjectPtr()->idx.kspace_encode_step_1;
int repetition = m1->getObjectPtr()->idx.kspace_encode_step_2;
// Enqueue incoming readouts and trajectories
//
frame_readout_queue_->enqueue_tail(duplicate_array(m2));
frame_traj_queue_->enqueue_tail(duplicate_array(m3));
// If the last readout for a slice has arrived then perform a reconstruction
//
bool is_last_scan_in_repetition =
m1->getObjectPtr()->isFlagSet(ISMRMRD::ISMRMRD_ACQ_LAST_IN_REPETITION);
if (is_last_scan_in_repetition) {
// Define the image header
//
GadgetContainerMessage<ISMRMRD::ImageHeader> *cm1 =
new GadgetContainerMessage<ISMRMRD::ImageHeader>();
GadgetContainerMessage< hoNDArray< std::complex<float> > > *cm2 =
new GadgetContainerMessage<hoNDArray< std::complex<float> > >();
cm1->getObjectPtr()->flags = 0;
cm1->cont(cm2);
cm1->getObjectPtr()->matrix_size[0] = dimensions_[0];
cm1->getObjectPtr()->matrix_size[1] = dimensions_[1];
cm1->getObjectPtr()->matrix_size[2] = 1;
cm1->getObjectPtr()->field_of_view[0] = field_of_view_[0];
cm1->getObjectPtr()->field_of_view[1] = field_of_view_[1];
cm1->getObjectPtr()->channels = num_coils_;
cm1->getObjectPtr()->repetition = m1->getObjectPtr()->idx.repetition;
memcpy(cm1->getObjectPtr()->position,
m1->getObjectPtr()->position,
sizeof(float)*3);
memcpy(cm1->getObjectPtr()->read_dir,
m1->getObjectPtr()->read_dir,
sizeof(float)*3);
memcpy(cm1->getObjectPtr()->phase_dir,
m1->getObjectPtr()->phase_dir,
sizeof(float)*3);
memcpy(cm1->getObjectPtr()->slice_dir,
m1->getObjectPtr()->slice_dir,
sizeof(float)*3);
memcpy(cm1->getObjectPtr()->patient_table_position,
m1->getObjectPtr()->patient_table_position, sizeof(float)*3);
cm1->getObjectPtr()->data_type = ISMRMRD::ISMRMRD_CXFLOAT;
cm1->getObjectPtr()->image_index = 0;
cm1->getObjectPtr()->image_series_index = 0;
//
// Perform reconstruction of repetition
//
// Get samples for frame
//
cuNDArray<float_complext> samples( extract_samples_from_queue( frame_readout_queue_.get()).get() );
// Get trajectories/dcw for frame
//
boost::shared_ptr< cuNDArray<floatd2> > traj(new cuNDArray<floatd2>);
boost::shared_ptr<cuNDArray<float> > dcw(new cuNDArray<float>);
extract_trajectory_and_dcw_from_queue( frame_traj_queue_.get(), traj.get(), dcw.get() );
//traj = compute_radial_trajectory_golden_ratio_2d<float>(samples_per_readout_,dimensions_[1],1,0,GR_ORIGINAL);
unsigned int num_profiles = samples.get_number_of_elements()/samples_per_readout_;
dcw = compute_radial_dcw_golden_ratio_2d<float>(samples_per_readout_,num_profiles,1.0,1.0f/samples_per_readout_/num_profiles,0,GR_ORIGINAL);
// Create output array
//
std::vector<size_t> img_dims(2);
img_dims[0] = dimensions_[0];
img_dims[1] = dimensions_[1];
cm2->getObjectPtr()->create(&img_dims);
cuNDArray<float_complext> image(&img_dims);
// Initialize plan
//
const float kernel_width = 5.5f;
cuNFFT_plan<float,2> plan( from_std_vector<size_t,2>(img_dims), from_std_vector<size_t,2>(img_dims)<<1, kernel_width );
plan.preprocess( traj.get(), cuNFFT_plan<float,2>::NFFT_PREP_NC2C );
/*
if( dcw->get_number_of_elements() == 0 ){
std::vector<size_t> dcw_dims; dcw_dims.push_back(samples_per_readout_);
hoNDArray<float> host_dcw( dcw_dims );
for( int i=0; i<(int)dcw_dims[0]; i++ )
host_dcw.get_data_ptr()[i]=abs(i-(int)dcw_dims[0]/2);
host_dcw.get_data_ptr()[dcw_dims[0]/2] = 0.25f; // ad hoc value (we do not want a DC component of 0)
dcw = expand(&host_dcw, traj->get_number_of_elements()/samples_per_readout_);
}
*/
// Gridder
//
plan.compute( &samples, &image,
(dcw->get_number_of_elements()>0) ? dcw.get() : 0x0,
cuNFFT_plan<float,2>::NFFT_BACKWARDS_NC2C );
// Download to host
//
image.to_host( (hoNDArray<float_complext>*)cm2->getObjectPtr() );
// Pass on data down the gadget chain
//
if (this->next()->putq(cm1) < 0) {
return GADGET_FAIL;
}
}
m1->release();
return GADGET_OK;
}
int gpuOsSenseGadget::process(GadgetContainerMessage<ISMRMRD::ImageHeader> *m1, GadgetContainerMessage<GenericReconJob> *m2)
{
// Is this data for this gadget's set/slice?
//
GDEBUG("Starting gpuOsSenseGadget\n");
if( m1->getObjectPtr()->set != set_number_ || m1->getObjectPtr()->slice != slice_number_ ) {
// No, pass it downstream...
return this->next()->putq(m1);
}
//GDEBUG("gpuOsSenseGadget::process\n");
//GPUTimer timer("gpuOsSenseGadget::process");
if (!is_configured_) {
GDEBUG("\nData received before configuration complete\n");
return GADGET_FAIL;
}
GenericReconJob* j = m2->getObjectPtr();
// Let's first check that this job has the required data...
if (!j->csm_host_.get() || !j->dat_host_.get() || !j->tra_host_.get() || !j->dcw_host_.get()) {
GDEBUG("Received an incomplete Sense job\n");
return GADGET_FAIL;
}
unsigned int samples = j->dat_host_->get_size(0);
unsigned int channels = j->dat_host_->get_size(1);
unsigned int rotations = samples / j->tra_host_->get_number_of_elements();
unsigned int frames = j->tra_host_->get_size(1)*rotations;
if( samples%j->tra_host_->get_number_of_elements() ) {
GDEBUG("Mismatch between number of samples (%d) and number of k-space coordinates (%d).\nThe first should be a multiplum of the latter.\n",
samples, j->tra_host_->get_number_of_elements());
return GADGET_FAIL;
}
boost::shared_ptr< cuNDArray<floatd2> > traj(new cuNDArray<floatd2> (j->tra_host_.get()));
boost::shared_ptr< cuNDArray<float> > dcw(new cuNDArray<float> (j->dcw_host_.get()));
sqrt_inplace(dcw.get());
boost::shared_ptr< cuNDArray<float_complext> > csm(new cuNDArray<float_complext> (j->csm_host_.get()));
boost::shared_ptr< cuNDArray<float_complext> > device_samples(new cuNDArray<float_complext> (j->dat_host_.get()));
// Take the reconstruction matrix size from the regulariaztion image.
// It could be oversampled from the sequence specified size...
matrix_size_ = uint64d2( j->reg_host_->get_size(0), j->reg_host_->get_size(1) );
cudaDeviceProp deviceProp;
if( cudaGetDeviceProperties( &deviceProp, device_number_ ) != cudaSuccess) {
GDEBUG( "\nError: unable to query device properties.\n" );
return GADGET_FAIL;
}
unsigned int warp_size = deviceProp.warpSize;
matrix_size_os_ =
uint64d2(((static_cast<unsigned int>(std::ceil(matrix_size_[0]*oversampling_factor_))+warp_size-1)/warp_size)*warp_size,
((static_cast<unsigned int>(std::ceil(matrix_size_[1]*oversampling_factor_))+warp_size-1)/warp_size)*warp_size);
GDEBUG("Matrix size : [%d,%d] \n", matrix_size_[0], matrix_size_[1]);
GDEBUG("Matrix size OS : [%d,%d] \n", matrix_size_os_[0], matrix_size_os_[1]);
std::vector<size_t> image_dims = to_std_vector(matrix_size_);
image_dims.push_back(frames);
E_->set_domain_dimensions(&image_dims);
E_->set_codomain_dimensions(device_samples->get_dimensions().get());
E_->set_csm(csm);
E_->setup( matrix_size_, matrix_size_os_, kernel_width_ );
E_->preprocess(traj.get());
{
auto precon = boost::make_shared<cuNDArray<float_complext>>(image_dims);
fill(precon.get(),float_complext(1.0f));
//solver_.set_preconditioning_image(precon);
}
reg_image_ = boost::shared_ptr< cuNDArray<float_complext> >(new cuNDArray<float_complext>(&image_dims));
// These operators need their domain/codomain set before being added to the solver
//
//E_->set_dcw(dcw);
GDEBUG("Prepared\n");
// Expand the average image to the number of frames
//
{
cuNDArray<float_complext> tmp(*j->reg_host_);
*reg_image_ = expand( tmp, frames );
}
PICS_->set_prior(reg_image_);
// Define preconditioning weights
//
//Apply weights
//*device_samples *= *dcw;
// Invoke solver
//
boost::shared_ptr< cuNDArray<float_complext> > result;
{
GDEBUG("Running NLCG solver\n");
GPUTimer timer("Running NLCG solver");
// Optionally, allow exclusive (per device) access to the solver
// This may not matter much in terms of speed, but it can in terms of memory consumption
//
if( exclusive_access_ )
_mutex[device_number_].lock();
result = solver_.solve(device_samples.get());
if( exclusive_access_ )
_mutex[device_number_].unlock();
}
// Provide some info about the scaling between the regularization and reconstruction.
// If it is not close to one, PICCS does not work optimally...
//
if( alpha_ > 0.0 ){
cuNDArray<float_complext> gpureg(j->reg_host_.get());
boost::shared_ptr< cuNDArray<float_complext> > gpurec = sum(result.get(),2);
*gpurec /= float(result->get_size(2));
float scale = abs(dot(gpurec.get(), gpurec.get())/dot(gpurec.get(),&gpureg));
GDEBUG("Scaling factor between regularization and reconstruction is %f.\n", scale);
}
if (!result.get()) {
GDEBUG("\nNon-linear conjugate gradient solver failed\n");
return GADGET_FAIL;
}
/*
static int counter = 0;
char filename[256];
sprintf((char*)filename, "recon_sb_%d.cplx", counter);
write_nd_array<float_complext>( sbresult->to_host().get(), filename );
counter++; */
// If the recon matrix size exceeds the sequence matrix size then crop
if( matrix_size_seq_ != matrix_size_ )
*result = crop<float_complext,2>( (matrix_size_-matrix_size_seq_)>>1, matrix_size_seq_, *result );
// Now pass on the reconstructed images
//
this->put_frames_on_que(frames,rotations,j,result.get(),channels);
frame_counter_ += frames;
m1->release();
return GADGET_OK;
}
int main( int argc, char** argv ){
#if (CISST_OS == CISST_LINUX_XENOMAI)
mlockall(MCL_CURRENT | MCL_FUTURE);
RT_TASK task;
rt_task_shadow( &task, "mtsWAMPDGCExample", 40, 0 );
#endif
cmnLogger::SetMask( CMN_LOG_ALLOW_ALL );
cmnLogger::SetMaskFunction( CMN_LOG_ALLOW_ALL );
cmnLogger::SetMaskDefaultLog( CMN_LOG_ALLOW_ALL );
if( argc != 3 ){
std::cout << "Usage: " << argv[0] << " can[0-1] GCMIP" << std::endl;
return -1;
}
std::string process( "Slave" );
mtsManagerLocal* taskManager = NULL;
try{ taskManager = mtsTaskManager::GetInstance( argv[2], process ); }
catch( ... ){
std::cerr << "Failed to connect to GCM: " << argv[2] << std::endl;
taskManager = mtsManagerLocal::GetInstance();
}
mtsKeyboard kb;
kb.SetQuitKey( 'q' );
kb.AddKeyWriteFunction( 'C', "PDGCEnable", "Enable", true );
kb.AddKeyWriteFunction( 'C', "TrajEnable", "Enable", true );
kb.AddKeyWriteFunction( 'M', "MoveEnable", "Enable", true );
taskManager->AddComponent( &kb );
// Initial configuration
vctDynamicVector<double> qinit( 7, 0.0 );
qinit[1] = -cmnPI_2;
qinit[3] = cmnPI-0.01;
qinit[5] = -cmnPI_2;
#if (CISST_OS == CISST_LINUX_XENOMAI)
osaRTSocketCAN can( argv[1], osaCANBus::RATE_1000 );
#else
osaSocketCAN can( argv[1], osaCANBus::RATE_1000 );
#endif
if( can.Open() != osaCANBus::ESUCCESS ){
CMN_LOG_RUN_ERROR << argv[0] << "Failed to open " << argv[1] << std::endl;
return -1;
}
mtsWAM WAM( "WAM", &can, osaWAM::WAM_7DOF, OSA_CPU4, 80 );
WAM.Configure();
WAM.SetPositions( qinit );
taskManager->AddComponent( &WAM );
cmnPath path;
path.AddRelativeToCisstShare("models/WAM");
// Rotate the base
vctMatrixRotation3<double> Rw0( 0.0, 0.0, -1.0,
0.0, 1.0, 0.0,
1.0, 0.0, 0.0 );
vctFixedSizeVector<double,3> tw0(0.0);
vctFrame4x4<double> Rtw0( Rw0, tw0 );
// Gain matrices
vctDynamicMatrix<double> Kp(7, 7, 0.0), Kd(7, 7, 0.0);
Kp[0][0] = 250; Kd[0][0] = 3.0;
Kp[1][1] = 250; Kd[1][1] = 3.0;
Kp[2][2] = 250; Kd[2][2] = 3.0;
Kp[3][3] = 200; Kd[3][3] = 3;
Kp[4][4] = 50; Kd[4][4] = 0.8;
Kp[5][5] = 50; Kd[5][5] = 0.8;
Kp[6][6] = 10; Kd[6][6] = .1;
mtsPDGC PDGC( "PDGC",
0.00125,
path.Find( "wam7cutter.rob" ),
Rtw0,
Kp,
Kd,
qinit,
OSA_CPU3 );
taskManager->AddComponent( &PDGC );
mtsTrajectory traj( "trajectory",
0.002,
path.Find( "wam7cutter.rob" ),
Rtw0,
qinit );
taskManager->AddComponent( &traj );
SetPoints setpoints( path.Find( "wam7cutter.rob" ), Rtw0, qinit );
taskManager->AddComponent( &setpoints );
// Connect the keyboard
if( !taskManager->Connect( kb.GetName(), "PDGCEnable",
PDGC.GetName(),"Control") ){
std::cout << "Failed to connect: "
<< kb.GetName() << "::PDGCEnable to "
<< PDGC.GetName() << "::Control" << std::endl;
return -1;
}
if( !taskManager->Connect( kb.GetName(), "TrajEnable",
traj.GetName(),"Control") ){
std::cout << "Failed to connect: "
<< kb.GetName() << "::TrajEnable to "
<< traj.GetName() << "::Control" << std::endl;
return -1;
}
if( !taskManager->Connect( kb.GetName(), "MoveEnable",
setpoints.GetName(),"Control") ){
std::cout << "Failed to connect: "
<< kb.GetName() << "::TrajEnable to "
<< setpoints.GetName() << "::Control" << std::endl;
return -1;
}
// connect the trajectory with setpoints
if( !taskManager->Connect( traj.GetName(), "Input",
setpoints.GetName(), "Output" ) ){
std::cout << "Failed to connect: "
<< traj.GetName() << "::Input to "
<< setpoints.GetName() << "::Output" << std::endl;
return -1;
}
// connect the trajectory to the controller
if( !taskManager->Connect( traj.GetName(), "Output",
PDGC.GetName(), "Input") ){
std::cout << "Failed to connect: "
<< traj.GetName() << "::Output to "
<< PDGC.GetName() << "::Input" << std::endl;
return -1;
}
// connect the controller to the WAM
if( !taskManager->Connect( WAM.GetName(), "Input",
PDGC.GetName(), "Output" ) ){
std::cout << "Failed to connect: "
<< WAM.GetName() << "::Input to "
<< PDGC.GetName() << "::Output" << std::endl;
return -1;
}
if( !taskManager->Connect( WAM.GetName(), "Output",
PDGC.GetName(), "Feedback" ) ){
std::cout << "Failed to connect: "
<< WAM.GetName() << "::Output to "
<< PDGC.GetName() << "::Feedback" << std::endl;
return -1;
}
taskManager->CreateAll();
taskManager->StartAll();
std::cout << "Press 'q' to exit." << std::endl;
pause();
return 0;
}
int gpuCgSpiritGadget::process(GadgetContainerMessage<ISMRMRD::ImageHeader> *m1, GadgetContainerMessage<GenericReconJob> *m2)
{
// Is this data for this gadget's set/slice?
//
if( m1->getObjectPtr()->set != set_number_ || m1->getObjectPtr()->slice != slice_number_ ) {
// No, pass it downstream...
return this->next()->putq(m1);
}
//GDEBUG("gpuCgSpiritGadget::process\n");
boost::shared_ptr<GPUTimer> process_timer;
if( output_timing_ )
process_timer = boost::shared_ptr<GPUTimer>( new GPUTimer("gpuCgSpiritGadget::process()") );
if (!is_configured_) {
GDEBUG("Data received before configuration was completed\n");
return GADGET_FAIL;
}
GenericReconJob* j = m2->getObjectPtr();
// Some basic validation of the incoming Spirit job
if (!j->csm_host_.get() || !j->dat_host_.get() || !j->tra_host_.get() || !j->dcw_host_.get() || !j->reg_host_.get()) {
GDEBUG("Received an incomplete Spirit job\n");
return GADGET_FAIL;
}
unsigned int samples = j->dat_host_->get_size(0);
unsigned int channels = j->dat_host_->get_size(1);
unsigned int rotations = samples / j->tra_host_->get_number_of_elements();
unsigned int frames = j->tra_host_->get_size(1)*rotations;
if( samples%j->tra_host_->get_number_of_elements() ) {
GDEBUG("Mismatch between number of samples (%d) and number of k-space coordinates (%d).\nThe first should be a multiplum of the latter.\n",
samples, j->tra_host_->get_number_of_elements());
return GADGET_FAIL;
}
boost::shared_ptr< cuNDArray<floatd2> > traj(new cuNDArray<floatd2> (j->tra_host_.get()));
boost::shared_ptr< cuNDArray<float> > dcw(new cuNDArray<float> (j->dcw_host_.get()));
sqrt_inplace(dcw.get()); //Take square root to use for weighting
boost::shared_ptr< cuNDArray<float_complext> > csm(new cuNDArray<float_complext> (j->csm_host_.get()));
boost::shared_ptr< cuNDArray<float_complext> > device_samples(new cuNDArray<float_complext> (j->dat_host_.get()));
cudaDeviceProp deviceProp;
if( cudaGetDeviceProperties( &deviceProp, device_number_ ) != cudaSuccess) {
GDEBUG( "Error: unable to query device properties.\n" );
return GADGET_FAIL;
}
unsigned int warp_size = deviceProp.warpSize;
matrix_size_ = uint64d2( j->reg_host_->get_size(0), j->reg_host_->get_size(1) );
matrix_size_os_ =
uint64d2(((static_cast<unsigned int>(std::ceil(matrix_size_[0]*oversampling_factor_))+warp_size-1)/warp_size)*warp_size,
((static_cast<unsigned int>(std::ceil(matrix_size_[1]*oversampling_factor_))+warp_size-1)/warp_size)*warp_size);
if( !matrix_size_reported_ ) {
GDEBUG("Matrix size : [%d,%d] \n", matrix_size_[0], matrix_size_[1]);
GDEBUG("Matrix size OS : [%d,%d] \n", matrix_size_os_[0], matrix_size_os_[1]);
matrix_size_reported_ = true;
}
std::vector<size_t> image_dims = to_std_vector(matrix_size_);
image_dims.push_back(frames);
image_dims.push_back(channels);
GDEBUG("Number of coils: %d %d \n",channels,image_dims.size());
E_->set_domain_dimensions(&image_dims);
E_->set_codomain_dimensions(device_samples->get_dimensions().get());
E_->set_dcw(dcw);
E_->setup( matrix_size_, matrix_size_os_, static_cast<float>(kernel_width_) );
E_->preprocess(traj.get());
boost::shared_ptr< cuNDArray<float_complext> > csm_device( new cuNDArray<float_complext>( csm.get() ));
S_->set_calibration_kernels(csm_device);
S_->set_domain_dimensions(&image_dims);
S_->set_codomain_dimensions(&image_dims);
/*
boost::shared_ptr< cuNDArray<float_complext> > reg_image(new cuNDArray<float_complext> (j->reg_host_.get()));
R_->compute(reg_image.get());
// Define preconditioning weights
boost::shared_ptr< cuNDArray<float> > _precon_weights = sum(abs_square(csm.get()).get(), 2);
boost::shared_ptr<cuNDArray<float> > R_diag = R_->get();
*R_diag *= float(kappa_);
*_precon_weights += *R_diag;
R_diag.reset();
reciprocal_sqrt_inplace(_precon_weights.get());
boost::shared_ptr< cuNDArray<float_complext> > precon_weights = real_to_complex<float_complext>( _precon_weights.get() );
_precon_weights.reset();
D_->set_weights( precon_weights );
*/
/*{
static int counter = 0;
char filename[256];
sprintf((char*)filename, "_traj_%d.real", counter);
write_nd_array<floatd2>( traj->to_host().get(), filename );
sprintf((char*)filename, "_dcw_%d.real", counter);
write_nd_array<float>( dcw->to_host().get(), filename );
sprintf((char*)filename, "_csm_%d.cplx", counter);
write_nd_array<float_complext>( csm->to_host().get(), filename );
sprintf((char*)filename, "_samples_%d.cplx", counter);
write_nd_array<float_complext>( device_samples->to_host().get(), filename );
sprintf((char*)filename, "_reg_%d.cplx", counter);
write_nd_array<float_complext>( reg_image->to_host().get(), filename );
counter++;
}*/
// Invoke solver
//
boost::shared_ptr< cuNDArray<float_complext> > cgresult;
{
boost::shared_ptr<GPUTimer> solve_timer;
if( output_timing_ )
solve_timer = boost::shared_ptr<GPUTimer>( new GPUTimer("gpuCgSpiritGadget::solve()") );
cgresult = cg_.solve(device_samples.get());
if( output_timing_ )
solve_timer.reset();
}
if (!cgresult.get()) {
GDEBUG("Iterative_spirit_compute failed\n");
return GADGET_FAIL;
}
/*
static int counter = 0;
char filename[256];
sprintf((char*)filename, "recon_%d.real", counter);
write_nd_array<float>( abs(cgresult.get())->to_host().get(), filename );
counter++;
*/
// If the recon matrix size exceeds the sequence matrix size then crop
if( matrix_size_seq_ != matrix_size_ )
cgresult = crop<float_complext,2>( (matrix_size_-matrix_size_seq_)>>1, matrix_size_seq_, cgresult.get() );
// Combine coil images
//
cgresult = real_to_complex<float_complext>(sqrt(sum(abs_square(cgresult.get()).get(), 3).get()).get()); // RSS
//cgresult = sum(cgresult.get(), 2);
// Pass on the reconstructed images
//
put_frames_on_que(frames,rotations,j,cgresult.get());
frame_counter_ += frames;
if( output_timing_ )
process_timer.reset();
m1->release();
return GADGET_OK;
}
std::vector<quad_common::QuadDesiredState> ModelPredictiveControlTrajectories::findOptimalTrajectory(nav_msgs::Odometry& initial_condition, nav_msgs::Odometry& previous_initial_condition_, std::vector<nav_msgs::Odometry> final_possible_conditions ){
//Define the trajectory starting state:
double dt = initial_condition.header.stamp.toSec() - previous_initial_condition_.header.stamp.toSec();
Vec3 pos0 = Vec3(initial_condition.pose.pose.position.x, initial_condition.pose.pose.position.y, initial_condition.pose.pose.position.z); //position
Vec3 vel0 = Vec3(initial_condition.twist.twist.linear.x, initial_condition.twist.twist.linear.y, initial_condition.twist.twist.linear.z); //velocity
Vec3 acc0 = Vec3((initial_condition.twist.twist.linear.x - previous_initial_condition_.twist.twist.linear.x)/dt,
(initial_condition.twist.twist.linear.y - previous_initial_condition_.twist.twist.linear.y)/dt,
(initial_condition.twist.twist.linear.z - previous_initial_condition_.twist.twist.linear.z)/dt); //acceleration
//Define how gravity lies in our coordinate system
Vec3 gravity = Vec3(0,0,-9.81);//[m/s**2]
//Define the state constraints. We'll only check that we don't fly into the floor:
Vec3 floorPos = Vec3(0,0,final_possible_conditions[0].pose.pose.position.z);//any point on the boundary
Vec3 floorNormal = Vec3(0,0,1);//we want to be in this direction of the boundary
RapidTrajectoryGenerator traj(pos0, vel0, acc0, gravity);
double min_cost_;
double optimal_time_;
bool set_first_optimal = false;
RapidTrajectoryGenerator optimal_trajectory_(pos0, vel0, acc0, gravity);
for(int i = 0; i < final_possible_conditions.size(); i++){
//define the goal state:
Vec3 posf = Vec3(final_possible_conditions[i].pose.pose.position.x,
final_possible_conditions[i].pose.pose.position.y,
final_possible_conditions[i].pose.pose.position.z + min_high_upon_base_); //position
Vec3 velf = Vec3(final_possible_conditions[i].twist.twist.linear.x,
final_possible_conditions[i].twist.twist.linear.y,
final_possible_conditions[i].twist.twist.linear.z); //velocity
//Vec3 velf = Vec3(0, 0, 0);
//leave final acceleration free
//Vec3 accf = Vec3(0, 0, 0); //acceleration
//define the duration:
double Tf = final_possible_conditions[i].header.stamp.toSec();
traj.SetGoalPosition(posf);
traj.SetGoalVelocity(velf);
//leave final acceleration free
//traj.SetGoalAcceleration(accf);
traj.Generate(Tf);
if((traj.CheckInputFeasibility(fmin, fmax, wmax, minTimeSec) == RapidTrajectoryGenerator::InputFeasible) &&
(traj.CheckPositionFeasibility(floorPos, floorNormal) == RapidTrajectoryGenerator::StateFeasible)){
double temp_cost = traj.GetCost();// + Tf;
if(!set_first_optimal){
min_cost_ = temp_cost;
optimal_trajectory_ = traj;
optimal_time_ = Tf;
set_first_optimal = true;
}
if(min_cost_ > temp_cost){
min_cost_ = temp_cost;
optimal_trajectory_ = traj;
optimal_time_ = Tf;
}
}
}
std::vector<quad_common::QuadDesiredState> trajectory;
double trajectory_dt = 0.1;
if(set_first_optimal){
trajectory = sampleTrajectory(optimal_trajectory_, optimal_time_, trajectory_dt);
visualizeTrajectory(trajectory);
}else{
trajectory = thirdOrderPolynomialTrajectory(initial_condition, final_possible_conditions[0], final_possible_conditions[0].header.stamp.toSec(),trajectory_dt);
visualizeTrajectory(trajectory);
/*quad_common::QuadDesiredState current_state;
current_state.position = Eigen::Vector3d(initial_condition.pose.pose.position.x + 2*trajectory_dt*final_possible_conditions[0].twist.twist.linear.x*(final_possible_conditions[0].pose.pose.position.x - initial_condition.pose.pose.position.x),
initial_condition.pose.pose.position.y + 2*trajectory_dt*final_possible_conditions[0].twist.twist.linear.y*(final_possible_conditions[0].pose.pose.position.y - initial_condition.pose.pose.position.y),
std::max(initial_condition.pose.pose.position.z,final_possible_conditions[0].pose.pose.position.z + 0.2));// + trajectory_dt*(final_possible_conditions[0].pose.pose.position.z - initial_condition.pose.pose.position.z));
current_state.velocity = Eigen::Vector3d(initial_condition.twist.twist.linear.x, initial_condition.twist.twist.linear.y, initial_condition.twist.twist.linear.z);
current_state.acceleration = Eigen::Vector3d((initial_condition.twist.twist.linear.x - previous_initial_condition_.twist.twist.linear.x)/dt,
(initial_condition.twist.twist.linear.y - previous_initial_condition_.twist.twist.linear.y)/dt,
(initial_condition.twist.twist.linear.z - previous_initial_condition_.twist.twist.linear.z)/dt);
current_state.yaw = 0.0;
trajectory.push_back(current_state);
trajectory.push_back(current_state);*/
}
return trajectory;
}
