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