int main(int argc, char** argv)
{
// Init the ROS node
ros::init(argc, argv, "robot_driver");
if (argc == 2) /// If the program was started with a trajectory parameter to set in an initial position the robo
{
std::string traj_filename(argv[1]);
//std::string traj_filename("/home/robot/code/iri/libbarrett/bin/test");
RobotArm arm;
// IMPORTANT: The robot must be at the start trajectory position
//arm.startJointsMove(arm.armAtHomePosition());
// Start the trajectory
control_msgs::FollowJointTrajectoryGoal trajectory_goal = arm.getProMpTrajectory(traj_filename);
arm.startJointsMove(arm.getJointsMovementSrvFromPoint(trajectory_goal.trajectory.points[0]));
arm.startTrajectory(trajectory_goal);
ROS_INFO("Action finished: %s\n", arm.getState().toString().c_str());
// Wait for trajectory completion
while(!arm.getState().isDone() && ros::ok())
{
usleep(50000);
}
}else{
std::cerr << "You must pass the path to the stored trajectory as an argument" << std::endl;
std::cerr << "Example rosrun iri_wam_controller test_simple_trajectory_promp /home/user/path/to/file" << std::endl;
return 0;
}
}
int main(int argc, char** argv) {
if(argc < 2) {
std::cerr << "Usage: " << argv[0] << " <LAMMPS trajectory"
<< "filename>" << std::endl;
return 1;
}
std::string traj_filename(argv[1]);
Trajectory t(traj_filename);
if(t.fail) {
std::cerr << "Couldn't open " << traj_filename
<< " for reading." << std::endl;
return 1;
}
int timesteps_read = 0;
std::ofstream ofile("temperatures.dat");
ofile << "#Timestep Tkin Tcon" << std::endl;
t.nextframe();
while(!t.fail) {
timesteps_read++;
std::cout << t.timestep << std::endl;
double tkin = 0.0;
double tcon1 = 0.0;
double tconF_numerator = 0.0;
double tconF_denominator = 0.0;
/******************************************************************************
!!! START MODIFYING HERE !!!
! the Trajectory object t (trajectory.h) contains the following
for you:
! int timestep
! int n (number of atoms)
! double box_lo[3] minimum coordinates in all three directions
! double box_hi[3] maximum coordinates in all three directions
! double prd[3] periodic replica distance in all three directions
! double x[N][3] atomic positons
! double v[N][3] atomic velocities
! double f[N][3] total force on each atom F_i
! the following functions are provided:
! - minimum_image(vector, prd) : apply the minimum image convention
! - veclengthsq(vector) : return the squared length of the vector x**2 + y**2 + z**2
! You need to give values to the following variables:
! - tkin (the degrees of freedom and mass are already accounted
for)
! - tconF_denominator
! - tconF_numerator
! the existing code will take care of the averages and standard deviations
******************************************************************************/
/******************************************************************************
!!! STOP MODIFYING HERE !!!
******************************************************************************/
tkin /= 3.0*mass*t.n;
tcon1 = tconF_denominator / tconF_numerator;
tkin_ave.push(tkin);
tcon1_ave.push(tcon1);
tconF_numerator_ave.push(tconF_numerator);
tconF_denominator_ave.push(tconF_denominator);
ofile << t.timestep << " " << tkin << " " << tcon1 <<
std::endl;
t.nextframe();
}
std::cout << "Temperatures" << std::endl;
std::cout << "Tkin " << tkin_ave.mean() << " +- " << tkin_ave.stderr() <<
std::endl;
std::cout << "Tcon1 " << tcon1_ave.mean() << " +- " <<
tcon1_ave.stderr() << std::endl;
double tconF_numerator = tconF_numerator_ave.mean();
double tconF_numerator_err = tconF_numerator_ave.stderr();
double tconF_denominator = tconF_denominator_ave.mean();
double tconF_denominator_err = tconF_denominator_ave.stderr();
double tconF = tconF_denominator / tconF_numerator;
double tconF_err =
tconF*tconF*(tconF_numerator_err*tconF_numerator_err/tconF_numerator/tconF_numerator
+
tconF_denominator_err*tconF_denominator_err/tconF_denominator/tconF_denominator);
std::cout << "TconF" << tconF << "+-" << tconF_err;
return 0;
}
