10 #include <boost/tokenizer.hpp>
12 #include <VirtualRobot/MathTools.h>
21 nlohmann::json& defaultConfig,
22 const std::string& entryName,
26 if (userConfig.find(entryName) != userConfig.end())
28 auto vec = userConfig[entryName].get<std::vector<float>>();
31 return Eigen::Map<Eigen::VectorXf>(vec.data(), vec.size());
35 return Eigen::VectorXf::Ones(size) *
value;
38 <<
" have size 0, fall back to default";
41 auto vec = defaultConfig[entryName].get<std::vector<float>>();
42 return Eigen::Map<Eigen::VectorXf>(vec.data(), vec.size());
46 getEigenMatrix(nlohmann::json& userConfig,
const std::string& entryName, Eigen::MatrixXf& mat)
48 if (userConfig.find(entryName) != userConfig.end())
50 auto vec = userConfig[entryName].get<std::vector<std::vector<float>>>();
51 int rows = vec.size();
52 int cols = vec[0].size();
56 mat.resize(rows, cols);
57 for (
int i = 0; i < rows; i++)
59 for (
int j = 0; j < cols; j++)
61 mat(i, j) = vec[i][j];
74 Eigen::VectorXf defaultValue,
75 const std::string& entryName)
77 if (userConfig.find(entryName) != userConfig.end())
79 auto vec = userConfig[entryName].get<std::vector<float>>();
80 if (vec.size() ==
static_cast<unsigned>(defaultValue.size()))
82 return Eigen::Map<Eigen::VectorXf>(vec.data(), vec.size());
84 ARMARX_WARNING <<
"size of the user defined parameter " << entryName
85 <<
" doesn't correspond to default size, use default value instead";
92 Eigen::Vector3d& targetPosition,
97 Eigen::Matrix4d currentPosed = currentPose.cast<
double>();
98 Eigen::Vector3d currentPosid = currentPosed.block<3, 1>(0, 3);
99 posiError = (currentPosid - targetPosition).
norm();
102 oriError = targetQuaternion.angularDistance(currentOrientation);
105 oriError = 2 *
M_PI - oriError;
112 Eigen::Vector3f rpy = VirtualRobot::MathTools::eigen4f2rpy(pose);
113 datafields[name +
"_x"] =
new Variant(pose(0, 3));
114 datafields[name +
"_y"] =
new Variant(pose(1, 3));
115 datafields[name +
"_z"] =
new Variant(pose(2, 3));
116 datafields[name +
"_rx"] =
new Variant(rpy(0));
117 datafields[name +
"_ry"] =
new Variant(rpy(1));
118 datafields[name +
"_rz"] =
new Variant(rpy(2));
124 for (
int i = 0; i < vec.size(); i++)
133 for (
size_t i = 0; i < vec.size(); i++)
142 j = nlohmann::json{{
"qw", fp.orientation->qw},
143 {
"qx", fp.orientation->qx},
144 {
"qy", fp.orientation->qy},
145 {
"qz", fp.orientation->qz},
146 {
"x", fp.position->x},
147 {
"y", fp.position->y},
148 {
"z", fp.position->z}};
154 j.at(
"qw").get_to(fp.orientation->qw);
155 j.at(
"qx").get_to(fp.orientation->qx);
156 j.at(
"qy").get_to(fp.orientation->qy);
157 j.at(
"qz").get_to(fp.orientation->qz);
158 j.at(
"x").get_to(fp.position->x);
159 j.at(
"y").get_to(fp.position->y);
160 j.at(
"z").get_to(fp.position->z);
173 to_json(nlohmann::json& j,
const PoseBasePtr& fp)
177 to_json(j, PosePtr::dynamicCast(fp));
206 Eigen::Matrix4f mat = VirtualRobot::MathTools::quat2eigen4f(
static_cast<float>(dvec[4]),
207 static_cast<float>(dvec[5]),
208 static_cast<float>(dvec[6]),
209 static_cast<float>(dvec[3]));
210 mat(0, 3) =
static_cast<float>(dvec[0]);
211 mat(1, 3) =
static_cast<float>(dvec[1]);
212 mat(2, 3) =
static_cast<float>(dvec[2]);
227 std::vector<double> result;
230 for (
size_t i = 0; i < 3; ++i)
232 result.at(i) =
static_cast<double>(mat(i, 3));
237 result.at(3) =
static_cast<double>(quat.w);
238 result.at(4) =
static_cast<double>(quat.x);
239 result.at(5) =
static_cast<double>(quat.y);
240 result.at(6) =
static_cast<double>(quat.z);
255 std::vector<float> result;
258 for (
size_t i = 0; i < 3; ++i)
260 result.at(i) = mat(i, 3);
265 result.at(3) = quat.w;
266 result.at(4) = quat.x;
267 result.at(5) = quat.y;
268 result.at(6) = quat.z;
277 for (
const double& d : dvec)
287 return Eigen::Map<Eigen::VectorXf>(vec.data(), vec.size());
291 sVecToString(
const std::vector<std::string>& vec,
const std::string& delimiter)
294 for (
size_t i = 0; i < vec.size(); ++i)
308 std::vector<double> timesteps;
309 std::vector<std::vector<double>> trajPoses;
311 std::ifstream f(filepath.c_str());
318 using Tokenizer = boost::tokenizer<boost::escaped_list_separator<char>>;
324 double current_time = 0;
326 std::vector<std::string> stringVec;
327 while (getline(f, line))
329 if (containsHeader && i == 1)
336 stringVec.assign(tok.begin(), tok.end());
338 if (stringVec.size() == 0)
340 std::cout <<
"Line " << i <<
" is invalid skipping: " << line << std::endl;
346 armarx::control::common::mp::arondto::MPTraj traj;
349 int size = stringVec.size() - 1;
354 timesteps.push_back(current_time);
355 current_time += 0.01;
356 size = stringVec.size();
361 timesteps.push_back(std::stod(stringVec[0]));
366 std::vector<double> currentPose;
368 for (
unsigned int k = j; k < stringVec.size(); ++k)
370 currentPose.push_back(std::stod(stringVec[k]));
374 trajPoses.push_back(currentPose);
380 if (timesteps.size() != trajPoses.size())
382 ARMARX_ERROR <<
"consistency error in reading trajectory";
384 armarx::control::common::mp::arondto::MPTraj taskTraj;
385 taskTraj.time.setZero(timesteps.size());
386 taskTraj.traj.setZero(timesteps.size(), trajPoses.at(0).size());
388 ARMARX_INFO <<
"traj pose dim " << trajPoses.at(0).size();
389 for (
unsigned int l = 0; l < timesteps.size(); l++)
391 taskTraj.time(l) = timesteps.at(l);
392 for (
size_t j = 0; j < trajPoses.at(0).size(); ++j)
393 taskTraj.traj(l, j) = trajPoses.at(l).at(j);