31 #include <SimoxUtility/json/eigen_conversion.h>
32 #include <SimoxUtility/json/json.hpp>
33 #include <VirtualRobot/math/Helpers.h>
44 #include <RobotAPI/interface/units/GraspCandidateProviderInterface.h>
54 Eigen::VectorXf vector(map.size());
56 map.begin(), map.end(), vector.data(), [](
const auto& item) { return item.second; });
64 RapidXmlWriter writer;
65 RapidXmlWriterNode root = writer.createRootNode(
"GraspTrajectory");
68 SimpleJsonLoggerEntry e;
69 e.Add(
"dt", keypoint->dt);
70 e.AddAsArr(
"Pose", keypoint->tcpTarget);
72 for (
const auto& [name,
value] : keypoint->handJointsTarget)
76 e.obj->add(
"HandValues", obj);
77 root.append_string_node(
"Keypoint", e.obj->toJsonString(0,
"",
true));
86 RapidXmlReaderNode
const root = reader->getRoot();
89 for (
const RapidXmlReaderNode& kpNode : root.nodes(
"Keypoint"))
91 StructuralJsonParser p(kpNode.value());
93 JPathNavigator nav(p.parsedJson);
94 float dt = nav.selectSingleNode(
"dt").asFloat();
97 std::vector<JPathNavigator> rows = nav.select(
"Pose/*");
98 for (
int i = 0; i < 4; i++)
100 std::vector<JPathNavigator> cells = rows.at(i).select(
"*");
101 for (
int j = 0; j < 4; j++)
103 pose(i, j) = cells.at(j).asFloat();
108 std::vector<JPathNavigator> cells = nav.select(
"HandValues/*");
109 for (
const auto& cell : cells)
111 handValues[cell.getKey()] = cell.asFloat();
115 const auto shapeNode = nav.selectSingleNode(
"shape");
118 std::optional<std::string> shape;
119 if (shapeNode.isString())
121 shape = nav.selectSingleNode(
"shape").asString();
128 traj = std::make_shared<GraspTrajectory>(pose, handValues, shape);
132 traj->addKeypoint(pose, handValues,
dt, shape);
142 const auto ext = std::filesystem::path(
filename).extension();
162 const std::optional<std::string> shape =
163 j.contains(
"shape") ? std::optional<std::string>(j.at(
"shape").get<std::string>())
166 const std::map<std::string, float> targetHandValues =
167 j.contains(
"HandValues") ? j.at(
"HandValues").get<std::map<std::string, float>>()
168 : std::map<std::string, float>{};
179 const nlohmann::json j = nlohmann::json::parse(ifs);
181 std::vector<GraspTrajectoryKeypoint> traj;
184 return std::make_shared<GraspTrajectory>(traj);
196 const std::optional<std::string>& shape)
200 keypointMap[0] = keypoints.size();
201 keypoints.push_back(keypoint);
206 if (keypointz.empty())
212 keypointMap[0] = keypoints.size();
213 keypoints.push_back(std::make_shared<Keypoint>(keypointz.front()));
216 for (std::size_t i = 1; i < keypointz.size(); i++)
218 const auto& kp = keypointz.at(i);
219 addKeypoint(kp.tcpTarget, kp.handJointsTarget, kp.dt, kp.shape);
227 const std::optional<std::string>& shape)
232 keypoint->updateVelocities(prev,
dt);
234 keypointMap[t] = keypoints.size();
235 keypoints.push_back(keypoint);
242 return keypoints.size();
245 const std::vector<GraspTrajectory::KeypointPtr>&
256 const std::optional<std::string>& shape)
259 if (index <= 0 || index > keypoints.size())
265 keypoint->updateVelocities(prev,
dt);
266 if (
index < keypoints.size())
269 next->updateVelocities(keypoint, next->dt);
271 keypoints.insert(keypoints.begin() +
index, keypoint);
279 if (index <= 0 || index >= keypoints.size())
283 keypoints.erase(keypoints.begin() +
index);
284 if (
index < keypoints.size())
289 next->updateVelocities(prev, next->dt);
299 const std::optional<std::string>& shape)
302 if (index <= 0 || index >= keypoints.size())
308 keypoint->updateVelocities(prev,
dt);
309 keypoints.at(
index) = keypoint;
317 if (index <= 0 || index >= keypoints.size())
323 keypoint->updateVelocities(prev,
dt);
331 return keypoints.at(keypoints.size() - 1);
338 return keypoints.at(i);
359 std::map<float, size_t>::const_iterator it, prev;
360 it = keypointMap.upper_bound(t);
361 if (it == keypointMap.end())
363 i1 = keypoints.size() - 1;
364 i2 = keypoints.size() - 1;
368 prev = std::prev(it);
371 f = ::math::Helpers::ILerp(prev->first, it->first, t);
381 return ::math::Helpers::Lerp(
393 if (oriVel.squaredNorm() == 0)
397 Eigen::AngleAxisf aa(oriVel.norm(), oriVel.normalized());
399 return aa.toRotationMatrix() *
getKeypoint(i1)->getTargetOrientation();
409 std::vector<Eigen::Matrix4f>
413 std::vector<Eigen::Matrix4f> res;
416 res.emplace_back(keypoint->getTargetPose());
421 std::vector<Eigen::Vector3f>
425 std::vector<Eigen::Vector3f> res;
428 res.emplace_back(keypoint->getTargetPosition());
433 std::vector<Eigen::Matrix3f>
437 std::vector<Eigen::Matrix3f> res;
440 res.emplace_back(keypoint->getTargetOrientation());
452 auto handTargetsMap =
getKeypoint(i1)->handJointsTarget;
455 const Eigen::VectorXf lerpTargets = handTargets1 * (1 - f) + handTargets2 * f;
456 auto* lerpTargetsIt = lerpTargets.data();
457 for (
auto& [name,
value] : handTargetsMap)
459 value = *(lerpTargetsIt++);
461 return handTargetsMap;
464 std::optional<std::string>
513 return getKeypoint(i2)->feedForwardHandJointsVelocity;
520 return keypointMap.rbegin()->first;
523 GraspTrajectory::Length
528 for (
size_t i = 1; i < keypoints.size(); i++)
533 Eigen::Vector3f pos0 = ::math::Helpers::GetPosition(k0->tcpTarget);
536 Eigen::Vector3f pos1 = ::math::Helpers::GetPosition(k1->tcpTarget);
539 l.pos += (pos1 - pos0).
norm();
540 l.ori += fabs(::math::Helpers::GetAngleAxisFromTo(ori0, ori1).
angle());
552 getKeypoint(0)->getTargetPose(), translation, rotation),
554 for (
size_t i = 1; i < keypoints.size(); i++)
558 ::math::Helpers::TranslateAndRotatePose(kp->getTargetPose(), translation, rotation),
559 kp->handJointsTarget,
571 for (
size_t i = 1; i < keypoints.size(); i++)
574 traj->addKeypoint(
transform * kp->getTargetPose(), kp->handJointsTarget, kp->dt);
588 const Eigen::Vector3f& handForward)
const
592 Eigen::Vector3f targetHandForward =
593 ::math::Helpers::TransformDirection(
target, handForward);
594 Eigen::Vector3f trajHandForward =
595 ::math::Helpers::TransformDirection(startPose, handForward);
596 Eigen::Vector3f up(0, 0, 1);
598 float angle = ::math::Helpers::Angle(targetHandForward, trajHandForward, up);
599 Eigen::AngleAxisf aa(
angle, up);
602 -::math::Helpers::GetPosition(startPose),
603 aa.toRotationMatrix(),
604 ::math::Helpers::GetPosition(
target));
613 flip_yz(0, 0) *= -1.0;
615 start_pose.block<3, 3>(0, 0) = flip_yz * start_pose.block<3, 3>(0, 0) * flip_yz;
622 target_pose.block<3, 3>(0, 0) = flip_yz * target_pose.block<3, 3>(0, 0) * flip_yz;
623 output_trajectory->addKeypoint(target_pose, kp->handJointsTarget, kp->dt);
625 return output_trajectory;
630 VirtualRobot::RobotNodePtr tcp,
639 GraspTrajectory::updateKeypointMap()
644 for (
size_t i = 0; i < keypoints.size(); i++)
656 Eigen::Vector3f pos0 = ::math::Helpers::GetPosition(prev->tcpTarget);
657 Eigen::Matrix3f ori0 = ::math::Helpers::GetOrientation(prev->tcpTarget);
660 Eigen::Vector3f pos1 = ::math::Helpers::GetPosition(
tcpTarget);
664 Eigen::Vector3f dpos = pos1 - pos0;
665 Eigen::Vector3f dori = ::math::Helpers::GetRotationVector(ori0, ori1);
666 Eigen::VectorXf dhnd = hnd1 - hnd0;
678 return ::math::Helpers::GetPosition(tcpTarget);
685 return ::math::Helpers::GetOrientation(tcpTarget);
699 const Eigen::Vector3f& feedForwardPosVelocity,
700 const Eigen::Vector3f& feedForwardOriVelocity,
701 const Eigen::VectorXf& feedForwardHandJointsVelocity,
702 const std::optional<std::string>& shape) :
703 tcpTarget(tcpTarget),
704 handJointsTarget(handJointsTarget),
707 feedForwardPosVelocity(feedForwardPosVelocity),
708 feedForwardOriVelocity(feedForwardOriVelocity),
709 feedForwardHandJointsVelocity(feedForwardHandJointsVelocity)
715 const std::optional<std::string>& shape) :
716 tcpTarget(tcpTarget),
717 handJointsTarget(handJointsTarget),
720 feedForwardPosVelocity(0, 0, 0),
721 feedForwardOriVelocity(0, 0, 0)