3 #include <VirtualRobot/MathTools.h>
11 tcp_control::arondto::MoveTCPToTargetPoseAcceptedType
12 GetDefaultParameterization()
14 tcp_control::arondto::MoveTCPToTargetPoseAcceptedType
ret;
15 ret.orientationalAccuracy = 0.01;
16 ret.positionalAccuracy = 25;
22 "MoveTCPToTargetPose",
23 "Move the TCP to a target pose",
26 tcp_control::arondto::MoveTCPToTargetPoseAcceptedType::ToAronType(),
27 GetDefaultParameterization().toAron()};
33 mixin::RobotReadingSkillMixin(mns),
34 PeriodicSimpleSpecializedSkill<tcp_control::arondto::MoveTCPToTargetPoseAcceptedType>(
41 MoveTCPToTargetPose::init(
const SpecializedInitInput& in)
45 tcpControlUnitRequestedOnStart =
true;
48 tcpControlUnitRequestedOnStart =
false;
56 PeriodicSkill::StepResult
57 MoveTCPToTargetPose::step(
const SpecializedMainInput& in)
62 VirtualRobot::RobotIO::RobotDescription::eStructure);
65 ARMARX_WARNING <<
"Unable to get robot from robotstatememory. Abort.";
69 std::string handRootNodeName =
"Hand L Base";
70 std::string robotAPIHandFileName =
"RobotAPI/robots/Armar3/ArmarIII-LeftHand.xml";
73 handRootNodeName =
"Hand R Base";
74 robotAPIHandFileName =
"RobotAPI/robots/Armar3/ArmarIII-RightHand.xml";
78 robot->getRobotNodeSet(in.parameters.kinematicChainName)->getTCP()->getName();
80 const Eigen::Matrix4f tcpPoseGlobal = robot->getRobotNode(tcpName)->getGlobalPose();
82 robot->getRobotNode(handRootNodeName)->getGlobalPose();
83 const Eigen::Matrix4f tcpInHandRoot = handRootPoseGlobal.inverse() * tcpPoseGlobal;
86 auto currentTCPPoseGlobal = robot->getRobotNode(tcpName)->getGlobalPose();
88 const auto tcpTargetPoseGlobal = in.parameters.targetPoseGlobal;
90 tcpTargetPoseGlobal * tcpInHandRoot.inverse();
92 const float baseSpeedFactor = 1.5;
93 const float maxTranslationSpeed = baseSpeedFactor * 30;
94 const float minTranslationSpeed = maxTranslationSpeed * 0.1;
95 const float maxRotationSpeed = baseSpeedFactor * 0.7;
96 const float minRotationSpeed = maxRotationSpeed * 0.1;
99 Eigen::Vector3f diffPos =
100 tcpTargetPoseGlobal.block<3, 1>(0, 3) - currentTCPPoseGlobal.block<3, 1>(0, 3);
101 float diffPosError = diffPos.norm();
103 float sig = 1 / (1 + pow(M_E, -diffPos.norm() / 5.f + 1.0));
104 diffPos = baseSpeedFactor * diffPos * sig;
107 if (diffPosError < minTranslationSpeed && diffPosError != 0)
109 diffPos *= (minTranslationSpeed / diffPosError);
112 else if (diffPosError > maxTranslationSpeed && diffPosError != 0)
114 diffPos *= (maxTranslationSpeed / diffPosError);
117 if (diffPosError < in.parameters.positionalAccuracy)
119 diffPos = Eigen::Vector3f::Zero();
127 a.pose(currentTCPPoseGlobal);
128 a.direction(diffPos);
129 a.length(diffPos.norm() * 3);
133 b.pose(tcpTargetPoseGlobal);
134 b.radius(diffPos.norm());
138 o.file(
"RobotAPI", robotAPIHandFileName);
139 o.pose(handRootTargetPoseGlobal);
145 Eigen::Vector3f angles(0, 0, 0);
150 currentTCPPoseGlobal.block<3, 3>(0, 0).inverse();
151 angles = VirtualRobot::MathTools::eigen3f2rpy(diffRot);
152 float anglesError = angles.norm();
163 if (anglesError < minRotationSpeed && anglesError != 0)
165 angles *= (minRotationSpeed / anglesError);
168 else if (anglesError > maxRotationSpeed && anglesError != 0)
170 angles *= (maxRotationSpeed / anglesError);
173 if (anglesError < in.parameters.orientationalAccuracy)
175 angles = Eigen::Vector3f::Zero();
188 auto cartesianError = diffPosError;
189 auto angleError = anglesError;
197 <<
"Current Thresholds: " << in.parameters.positionalAccuracy <<
"mm, "
198 << in.parameters.orientationalAccuracy * 180.0f /
M_PI <<
"deg";
201 <<
", " << angleVelocity.y <<
", " << angleVelocity.z <<
")\n"
202 <<
"catesian vel: " << cartesianVelocity.x <<
", " << cartesianVelocity.y
203 <<
", " << cartesianVelocity.z <<
")";
205 if (angleError <= in.parameters.orientationalAccuracy &&
206 cartesianError <= in.parameters.positionalAccuracy)
219 MoveTCPToTargetPose::exit(
const SpecializedExitInput& in)
226 VirtualRobot::RobotIO::RobotDescription::eStructure);
229 ARMARX_WARNING <<
"Unable to get robot from robotstatememory. Abort exit method.";
233 std::string tcpName =
234 robot->getRobotNodeSet(in.parameters.kinematicChainName)->getTCP()->getName();
238 if (!tcpControlUnitRequestedOnStart)