MoveTCPToTargetPose.cpp
Go to the documentation of this file.
1 #include "MoveTCPToTargetPose.h"
2 
3 #include <VirtualRobot/MathTools.h>
4 
5 #include "util/TCPControlUtil.h"
6 
7 namespace armarx::skills
8 {
9  namespace
10  {
11  tcp_control::arondto::MoveTCPToTargetPoseAcceptedType
12  GetDefaultParameterization()
13  {
14  tcp_control::arondto::MoveTCPToTargetPoseAcceptedType ret;
15  ret.orientationalAccuracy = 0.01;
16  ret.positionalAccuracy = 25;
17  return ret;
18  }
19  } // namespace
20 
21  SkillDescription MoveTCPToTargetPose::Description = skills::SkillDescription{
22  "MoveTCPToTargetPose",
23  "Move the TCP to a target pose",
24  {},
26  tcp_control::arondto::MoveTCPToTargetPoseAcceptedType::ToAronType(),
27  GetDefaultParameterization().toAron()};
28 
30  armarx::viz::Client& arviz,
31  TCPControlSkillContext& context) :
32  TCPControlSkill(mns, arviz, Description.skillName, context),
33  mixin::RobotReadingSkillMixin(mns),
34  PeriodicSimpleSpecializedSkill<tcp_control::arondto::MoveTCPToTargetPoseAcceptedType>(
35  Description,
36  armarx::core::time::Frequency::Hertz(100))
37  {
38  }
39 
41  MoveTCPToTargetPose::init(const SpecializedInitInput& in)
42  {
44 
45  tcpControlUnitRequestedOnStart = true;
46  if (!context.tcpControlUnitPrx->isRequested())
47  {
48  tcpControlUnitRequestedOnStart = false;
49  context.tcpControlUnitPrx->request();
50  }
51  context.tcpControlUnitPrx->setCycleTime(100);
52 
53  return {.status = TerminatedSkillStatus::Succeeded};
54  }
55 
56  PeriodicSkill::StepResult
57  MoveTCPToTargetPose::step(const SpecializedMainInput& in)
58  {
59  auto robot =
60  robotReader.getSynchronizedRobot(in.parameters.robotName,
62  VirtualRobot::RobotIO::RobotDescription::eStructure);
63  if (!robot)
64  {
65  ARMARX_WARNING << "Unable to get robot from robotstatememory. Abort.";
66  return {ActiveOrTerminatedSkillStatus::Failed, nullptr};
67  }
68 
69  std::string handRootNodeName = "Hand L Base";
70  std::string robotAPIHandFileName = "RobotAPI/robots/Armar3/ArmarIII-LeftHand.xml";
71  if (simox::alg::starts_with(in.parameters.kinematicChainName, "Right"))
72  {
73  handRootNodeName = "Hand R Base";
74  robotAPIHandFileName = "RobotAPI/robots/Armar3/ArmarIII-RightHand.xml";
75  }
76 
77  std::string tcpName =
78  robot->getRobotNodeSet(in.parameters.kinematicChainName)->getTCP()->getName();
79 
80  const Eigen::Matrix4f tcpPoseGlobal = robot->getRobotNode(tcpName)->getGlobalPose();
81  const Eigen::Matrix4f handRootPoseGlobal =
82  robot->getRobotNode(handRootNodeName)->getGlobalPose();
83  const Eigen::Matrix4f tcpInHandRoot = handRootPoseGlobal.inverse() * tcpPoseGlobal;
84 
85  // get current pose of tcp
86  auto currentTCPPoseGlobal = robot->getRobotNode(tcpName)->getGlobalPose();
87 
88  const auto tcpTargetPoseGlobal = in.parameters.targetPoseGlobal;
89  const Eigen::Matrix4f handRootTargetPoseGlobal =
90  tcpTargetPoseGlobal * tcpInHandRoot.inverse();
91 
92  const float baseSpeedFactor = 1.5;
93  const float maxTranslationSpeed = baseSpeedFactor * 30;
94  const float minTranslationSpeed = maxTranslationSpeed * 0.1; // 10% of max
95  const float maxRotationSpeed = baseSpeedFactor * 0.7;
96  const float minRotationSpeed = maxRotationSpeed * 0.1; // 10% of max
97 
98  // get translational difference
99  Eigen::Vector3f diffPos =
100  tcpTargetPoseGlobal.block<3, 1>(0, 3) - currentTCPPoseGlobal.block<3, 1>(0, 3);
101  float diffPosError = diffPos.norm();
102 
103  float sig = 1 / (1 + pow(M_E, -diffPos.norm() / 5.f + 1.0)); // sigmoid
104  diffPos = baseSpeedFactor * diffPos * sig;
105 
106  // Dont let it go to slow
107  if (diffPosError < minTranslationSpeed && diffPosError != 0)
108  {
109  diffPos *= (minTranslationSpeed / diffPosError);
110  }
111 
112  else if (diffPosError > maxTranslationSpeed && diffPosError != 0)
113  {
114  diffPos *= (maxTranslationSpeed / diffPosError);
115  }
116 
117  if (diffPosError < in.parameters.positionalAccuracy)
118  {
119  diffPos = Eigen::Vector3f::Zero();
120  }
121 
122  FramedDirection cartesianVelocity(diffPos, GlobalFrame, "");
123 
124  {
125  auto l = arviz.layer(layerName);
126  auto a = armarx::viz::Arrow("cartesianVelocity");
127  a.pose(currentTCPPoseGlobal);
128  a.direction(diffPos);
129  a.length(diffPos.norm() * 3);
130  l.add(a);
131 
132  auto b = armarx::viz::Sphere("targetPose");
133  b.pose(tcpTargetPoseGlobal);
134  b.radius(diffPos.norm());
135  l.add(b);
136 
137  auto o = armarx::viz::Robot("grasp");
138  o.file("RobotAPI", robotAPIHandFileName);
139  o.pose(handRootTargetPoseGlobal);
140  l.add(o);
141 
142  arviz.commit(l);
143  }
144 
145  Eigen::Vector3f angles(0, 0, 0);
146  //float axisRot = 0;
147 
148  // get rotational difference
149  Eigen::Matrix3f diffRot = tcpTargetPoseGlobal.block<3, 3>(0, 0) *
150  currentTCPPoseGlobal.block<3, 3>(0, 0).inverse();
151  angles = VirtualRobot::MathTools::eigen3f2rpy(diffRot);
152  float anglesError = angles.norm();
153 
154  ARMARX_IMPORTANT << VAROUT(angles);
155 
156  //rotation diff around one axis
157  //Eigen::Matrix4f diffPose = Eigen::Matrix4f::Identity();
158  //diffPose.block<3, 3>(0, 0) = diffRot;
159  //Eigen::Vector3f axis;
160  //VirtualRobot::MathTools::eigen4f2axisangle(diffPose, axis, axisRot);
161 
162  // Check velocities
163  if (anglesError < minRotationSpeed && anglesError != 0)
164  {
165  angles *= (minRotationSpeed / anglesError);
166  }
167 
168  else if (anglesError > maxRotationSpeed && anglesError != 0)
169  {
170  angles *= (maxRotationSpeed / anglesError);
171  }
172 
173  if (anglesError < in.parameters.orientationalAccuracy)
174  {
175  angles = Eigen::Vector3f::Zero();
176  }
177 
178  FramedDirection angleVelocity(angles, GlobalFrame, "");
179 
180  // // make sure we don't move the hand too low
181  // if (handPoseFromKinematicModelEigen(2,3) < 1000 && cartesianVelocity->z < 0)
182  // {
183  // ARMARX_IMPORTANT << "Hand too low, moving it up a bit";
184  // if (handPoseFromKinematicModelEigen(2,3) < 950) cartesianVelocity->z = 0.1f*maxSpeed;
185  // else cartesianVelocity->z = 0.05f*maxSpeed;
186  // }
187 
188  auto cartesianError = diffPosError;
189  auto angleError = anglesError; // axisRot
190 
191  ARMARX_INFO << deactivateSpam(0.5) << "Distance to target: "
192  << (std::to_string(cartesianError) + " mm, " +
193  std::to_string(angleError * 180 / M_PI) + " deg")
194  << ", \n"
195  << "Elapsed time: "
196  << (armarx::core::time::DateTime::Now() - started).toMilliSeconds() << "\n"
197  << "Current Thresholds: " << in.parameters.positionalAccuracy << "mm, "
198  << in.parameters.orientationalAccuracy * 180.0f / M_PI << "deg";
199 
200  ARMARX_INFO << deactivateSpam(0.5) << "Request to move TCP: angle vel: (" << angleVelocity.x
201  << ", " << angleVelocity.y << ", " << angleVelocity.z << ")\n"
202  << "catesian vel: " << cartesianVelocity.x << ", " << cartesianVelocity.y
203  << ", " << cartesianVelocity.z << ")";
204 
205  if (angleError <= in.parameters.orientationalAccuracy &&
206  cartesianError <= in.parameters.positionalAccuracy)
207  {
209  }
210 
211  context.tcpControlUnitPrx->setTCPVelocity(in.parameters.kinematicChainName,
212  tcpName,
213  new FramedDirection(cartesianVelocity),
214  new FramedDirection(angleVelocity));
215  return {ActiveOrTerminatedSkillStatus::Running, nullptr};
216  }
217 
218  Skill::ExitResult
219  MoveTCPToTargetPose::exit(const SpecializedExitInput& in)
220  {
221  clearLayer();
222 
223  auto robot =
224  robotReader.getSynchronizedRobot(in.parameters.robotName,
226  VirtualRobot::RobotIO::RobotDescription::eStructure);
227  if (!robot)
228  {
229  ARMARX_WARNING << "Unable to get robot from robotstatememory. Abort exit method.";
230  return {.status = TerminatedSkillStatus::Failed};
231  }
232 
233  std::string tcpName =
234  robot->getRobotNodeSet(in.parameters.kinematicChainName)->getTCP()->getName();
235 
236  stopTCPMovement(in.parameters.kinematicChainName, tcpName);
237 
238  if (!tcpControlUnitRequestedOnStart)
239  {
240  context.tcpControlUnitPrx->release();
241  }
242 
243  return {.status = TerminatedSkillStatus::Succeeded};
244  }
245 } // namespace armarx::skills
armarx::skills::TCPControlSkill::stopTCPMovement
void stopTCPMovement(const std::string &kinematicChainName, const std::string &tcpName)
Definition: TCPControlSkill.h:70
armarx::viz::Client::commit
CommitResult commit(StagedCommit const &commit)
Definition: Client.cpp:89
armarx::skills::TCPControlSkillContext
Definition: TCPControlSkill.h:45
armarx::aron::ret
ReaderT::InputType T & ret
Definition: rw.h:13
armarx::skills::TerminatedSkillStatus::Succeeded
@ Succeeded
ARMARX_IMPORTANT
#define ARMARX_IMPORTANT
Definition: Logging.h:190
armarx::skills::ActiveOrTerminatedSkillStatus::Succeeded
@ Succeeded
armarx::skills::ActiveOrTerminatedSkillStatus::Failed
@ Failed
armarx::skills
This file is part of ArmarX.
Definition: PeriodicUpdateWidget.cpp:11
GfxTL::Matrix4f
MatrixXX< 4, 4, float > Matrix4f
Definition: MatrixXX.h:650
armarx::core::time::DateTime::Now
static DateTime Now()
Definition: DateTime.cpp:51
TCPControlUtil.h
armarx::skills::mixin::RobotReadingSkillMixin::robotReader
armem::robot_state::VirtualRobotReader robotReader
Definition: RobotReadingSkillMixin.h:11
armarx::skills::MoveTCPToTargetPose::MoveTCPToTargetPose
MoveTCPToTargetPose(armem::client::MemoryNameSystem &mns, armarx::viz::Client &arviz, TCPControlSkillContext &context)
Definition: MoveTCPToTargetPose.cpp:29
armarx::GlobalFrame
const std::string GlobalFrame
Definition: FramedPose.h:65
armarx::core::time::Frequency
Represents a frequency.
Definition: Frequency.h:16
armarx::viz::Arrow
Definition: Elements.h:196
armarx::armem::robot_state::RobotReader::connect
virtual void connect(armem::client::MemoryNameSystem &memoryNameSystem)
Definition: RobotReader.cpp:49
armarx::skills::TCPControlSkillContext::tcpControlUnitPrx
TCPControlUnitInterfacePrx tcpControlUnitPrx
Definition: TCPControlSkill.h:48
armarx::starts_with
bool starts_with(const std::string &haystack, const std::string &needle)
Definition: StringHelpers.cpp:47
armarx::viz::Sphere
Definition: Elements.h:133
armarx::skills::mixin::ArvizSkillMixin::arviz
armarx::viz::Client arviz
Definition: ArvizSkillMixin.h:11
deactivateSpam
SpamFilterDataPtr deactivateSpam(SpamFilterDataPtr const &spamFilter, float deactivationDurationSec, const std::string &identifier, bool deactivate)
Definition: Logging.cpp:75
armarx::ctrlutil::a
double a(double t, double a0, double j)
Definition: CtrlUtil.h:45
M_PI
#define M_PI
Definition: MathTools.h:17
armarx::armem::human::Robot
@ Robot
Definition: util.h:17
armarx::skills::mixin::ArvizSkillMixin::layerName
std::string layerName
Definition: ArvizSkillMixin.h:12
armarx::skills::TCPControlSkill::context
TCPControlSkillContext & context
Definition: TCPControlSkill.h:83
armarx::armem::robot_state::VirtualRobotReader::getSynchronizedRobot
VirtualRobot::RobotPtr getSynchronizedRobot(const std::string &name, const VirtualRobot::RobotIO::RobotDescription &loadMode=VirtualRobot::RobotIO::RobotDescription::eStructure, bool blocking=true)
armarx::to_string
const std::string & to_string(const std::string &s)
Definition: StringHelpers.h:41
armarx::skills::ActiveOrTerminatedSkillStatus::Running
@ Running
armarx::skills::TerminatedSkillStatus::Failed
@ Failed
ARMARX_INFO
#define ARMARX_INFO
Definition: Logging.h:181
armarx::VariantType::FramedDirection
const VariantTypeId FramedDirection
Definition: FramedPose.h:37
armarx::skills::mixin::ArvizSkillMixin::clearLayer
void clearLayer()
Definition: ArvizSkillMixin.h:20
armarx::skills::TCPControlSkill
Definition: TCPControlSkill.h:59
VAROUT
#define VAROUT(x)
Definition: StringHelpers.h:198
armarx::skills::MoveTCPToTargetPose::Description
static SkillDescription Description
Definition: MoveTCPToTargetPose.h:53
armarx::skills::Skill::InitResult
A result struct for skill initialization.
Definition: Skill.h:27
armarx::armem::client::MemoryNameSystem
The memory name system (MNS) client.
Definition: MemoryNameSystem.h:68
GfxTL::Matrix3f
MatrixXX< 3, 3, float > Matrix3f
Definition: MatrixXX.h:649
ARMARX_WARNING
#define ARMARX_WARNING
Definition: Logging.h:193
armarx::viz::Client::layer
Layer layer(std::string const &name) const
Definition: Client.cpp:80
armarx::viz::Client
Definition: Client.h:117
armarx
This file offers overloads of toIce() and fromIce() functions for STL container types.
Definition: ArmarXTimeserver.cpp:27
MoveTCPToTargetPose.h
armarx::core::time::Duration::MilliSeconds
static Duration MilliSeconds(std::int64_t milliSeconds)
Constructs a duration in milliseconds.
Definition: Duration.cpp:48