6 #ifndef __sot_torque_control_trajectory_generator_H__ 7 #define __sot_torque_control_trajectory_generator_H__ 14 #if defined(trajectory_generator_EXPORTS) 15 #define TRAJECTORYGENERATOR_EXPORT __declspec(dllexport) 17 #define TRAJECTORYGENERATOR_EXPORT __declspec(dllimport) 20 #define HRP2COMMON_EXPORT 31 #include "boost/assign.hpp" 32 #include <dynamic-graph/signal-helper.h> 39 #define MAXBUFSIZE ((int)1000000) 42 int cols = 0, rows = 0;
47 infile.open(filename);
48 while (!infile.eof()) {
50 getline(infile, line);
54 std::stringstream stream(line);
55 while (!stream.eof()) stream >> buff[cols * rows + temp_cols++];
57 if (temp_cols == 0)
continue;
61 else if (temp_cols != cols && !infile.eof()) {
62 std::cout <<
"Error while reading matrix from file, line " << rows <<
" has " << temp_cols
63 <<
" columnds, while preceding lines had " << cols <<
" columnds\n";
64 std::cout << line <<
"\n";
70 std::cout <<
"Max buffer size exceeded (" << rows <<
" rows, " << cols <<
" cols)\n";
78 Eigen::MatrixXd result(rows, cols);
79 for (
int i = 0; i < rows; i++)
80 for (
int j = 0; j < cols; j++) result(i, j) = buff[cols * i + j];
107 void sendMsg(
const std::string& msg, MsgType t = MSG_TYPE_INFO,
const char* file =
"",
int line = 0) {
108 sendMsg(
"[AbstrTrajGen] " + msg, t, file, line);
120 const Eigen::VectorXd& x_final) {
121 assert(x_init.size() == x_final.size() &&
"Initial and final state must have the same size");
130 if (x_init.size() !=
m_x_init.size())
return false;
139 if (1 !=
m_x_init.size())
return false;
148 if (x_final.size() !=
m_x_final.size())
return false;
160 if (traj_time <= 0.0)
return false;
205 if (data.cols() != 3 *
m_size) {
206 std::cout <<
"Unexpected number of columns (expected " << 3 *
m_size <<
", found " << data.cols() <<
")\n";
223 Eigen::VectorXd::Index i = (Eigen::VectorXd::Index)std::floor(
m_t /
m_dt);
243 double td2 = td * td;
244 double td3 = td2 * td;
245 double td4 = td3 * td;
246 double td5 = td4 * td;
247 double p = 10 * td3 - 15 * td4 + 6 * td5;
248 double dp = (30 * td2 - 60 * td3 + 30 * td4) /
m_traj_time;
269 double two_pi_f = 2 * M_PI * f;
270 double two_pi_f_t = two_pi_f *
m_t;
271 double p = 0.5 * (1.0 - cos(two_pi_f_t));
272 double dp = 0.5 * two_pi_f * sin(two_pi_f_t);
273 double ddp = 0.5 * two_pi_f * two_pi_f * cos(two_pi_f_t);
299 if (Tacc < 0.0 || Tacc > 0.5 *
m_traj_time)
return false;
321 m_ddx = 0.0 * max_vel;
322 m_dx = way * max_vel;
347 if (!res)
return false;
410 if (f0.size() !=
m_f0.size())
return false;
416 if (1 !=
m_f0.size())
return false;
422 if (f1.size() !=
m_f1.size())
return false;
428 if (1 !=
m_f1.size())
return false;
446 m_p = 0.5 * (1.0 -
m_phi.array().cos());
448 m_ddp = 2.0 * M_PI * M_PI *
m_f.array() *
m_f.array() *
m_phi.array().cos();
463 #endif // #ifndef __sot_torque_control_trajectory_generators_H__
const Eigen::VectorXd & compute_next_point()
bool set_acceleration_time(const double Tacc)
virtual bool isTrajectoryEnded()
virtual const Eigen::VectorXd & getPos()
const Eigen::VectorXd & compute_next_point()
double m_t
control dt (sampling period of the trajectory)
const Eigen::VectorXd & compute_next_point()
virtual const Eigen::VectorXd & compute_next_point()
virtual void resizeAllData(Eigen::VectorXd::Index size)
virtual bool set_final_frequency(const Eigen::VectorXd &f1)
virtual bool set_initial_frequency(const Eigen::VectorXd &f0)
virtual bool set_initial_frequency(const double &f0)
TriangleTrajectoryGenerator(double dt, double traj_time, int size)
virtual bool set_final_point(const Eigen::VectorXd &x_final)
Eigen::VectorXd m_x_init
current acceleration
virtual bool isTrajectoryEnded()
Eigen::VectorXd m_phi
current frequency (i.e. time derivative of the phase over 2*pi)
Eigen::MatrixXd readMatrixFromFile(const char *filename)
void sendMsg(const std::string &msg, MsgType t=MSG_TYPE_INFO, const char *file="", int line=0)
virtual bool set_initial_point(const double &x_init)
Eigen::VectorXd m_ddx
current velocity
virtual bool set_final_point(const double &x_final)
virtual bool isTrajectoryEnded()
Eigen::VectorXd::Index m_size
current time
Eigen::MatrixXd m_velTraj
virtual bool isTrajectoryEnded()
SinusoidTrajectoryGenerator(double dt, double traj_time, int size)
Eigen::VectorXd m_phi_0
current phase
virtual const Eigen::VectorXd & getVel()
Eigen::VectorXd m_dx
current position
virtual bool set_trajectory_time(double traj_time)
AbstractTrajectoryGenerator(double dt, double traj_time, const Eigen::VectorXd &x_init, const Eigen::VectorXd &x_final)
MinimumJerkTrajectoryGenerator(double dt, double traj_time, int size)
Eigen::MatrixXd m_posTraj
const Eigen::VectorXd & compute_next_point()
virtual const Eigen::VectorXd & get_initial_point()
ConstantAccelerationTrajectoryGenerator(double dt, double traj_time, int size)
virtual const Eigen::VectorXd & getAcc()
virtual bool set_trajectory_time(double traj_time)
virtual const Eigen::VectorXd & get_final_point()
virtual bool loadTextFile(const std::string &fileName)
TextFileTrajectoryGenerator(double dt, Eigen::VectorXd::Index size)
Eigen::VectorXd m_k
final frequency
NoTrajectoryGenerator(int size)
AbstractTrajectoryGenerator(double dt, double traj_time, Eigen::VectorXd::Index size)
double m_dt
time to go from x_init to x_final (sec)
Eigen::VectorXd m_x_final
initial position
virtual const Eigen::VectorXd & compute_next_point()=0
double m_traj_time
final position
Eigen::MatrixXd m_accTraj
virtual bool set_initial_point(const Eigen::VectorXd &x_init)
virtual const Eigen::VectorXd & compute_next_point()
Eigen::VectorXd m_p
phase shift for second half of trajectory
Eigen::VectorXd m_f1
initial frequency
LinearChirpTrajectoryGenerator(double dt, double traj_time, int size)
virtual bool set_final_frequency(const double &f1)
virtual const Eigen::VectorXd & compute_next_point()
Eigen::VectorXd m_f
frequency first derivative
virtual bool isTrajectoryEnded()