CalculateForceControlRobotPose.cpp
Go to the documentation of this file.
1 /*
2  * This file is part of ArmarX.
3  *
4  * Copyright (C) 2014-2016, High Performance Humanoid Technologies (H2T), Karlsruhe Institute of Technology (KIT), all rights reserved.
5  *
6  * ArmarX is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License version 2 as
8  * published by the Free Software Foundation.
9  *
10  * ArmarX is distributed in the hope that it will be useful, but
11  * WITHOUT ANY WARRANTY; without even the implied warranty of
12  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
13  * GNU General Public License for more details.
14  *
15  * You should have received a copy of the GNU General Public License
16  * along with this program. If not, see <http://www.gnu.org/licenses/>.
17  *
18  * @package RobotSkillTemplates::CalculateForceControlRobotPoseGroup
19  * @author [Author Name] ( [Author Email] )
20  * @date 2014
21  * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
22  * GNU General Public License
23  */
24 
25 #include "CalculateForceControlRobotPose.h"
26 #include <VirtualRobot/MathTools.h>
27 
28 #include <ArmarXCore/core/time/TimeUtil.h>
29 
30 #include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
31 
32 
33 using namespace armarx;
34 using namespace CoupledInteractionGroup;
35 
36 // DO NOT EDIT NEXT LINE
37 CalculateForceControlRobotPose::SubClassRegistry
38  CalculateForceControlRobotPose::Registry(CalculateForceControlRobotPose::GetName(),
39  &CalculateForceControlRobotPose::CreateInstance);
40 
41 CalculateForceControlRobotPose::CalculateForceControlRobotPose(
42  XMLStateConstructorParams stateData) :
43  XMLStateTemplate<CalculateForceControlRobotPose>(stateData),
44  CalculateForceControlRobotPoseGeneratedBase<CalculateForceControlRobotPose>(stateData)
45 {
46 }
47 
48 void
49 CalculateForceControlRobotPose::onEnter()
50 {
51 
52  CoupledInteractionGroupStatechartContext* context =
53  getContext<CoupledInteractionGroupStatechartContext>();
54  int kernelSize = 50;
55  std::vector<Vector3Ptr> platformVelMeanFilter;
56 
57  if (local.isPlatformVelMeanFilterSet())
58  {
59  platformVelMeanFilter = local.getPlatformVelMeanFilter();
60  }
61 
62  // put your user code for the enter-point here
63  // execution time should be short (<100ms)
64 
65  std::string leftTcpName = in.getLeftHandName();
66  std::string rightTcpName = in.getRightHandName();
67  //leftTcpName = std::string("Wrist 2 L");
68  //rightTcpName = std::string("Wrist 2 R");
69  auto now = TimeUtil::GetTime();
70  IceUtil::Time duration;
71  duration = now - in.getCurrentTimestamp()->toTime();
72  int counter = in.getcounter();
73  counter++;
74  //else
75  FramedPosePtr desiredLeftTcpPose = in.getDesiredLeftTcpPose();
76  FramedPosePtr desiredRightTcpPose = in.getDesiredRightTcpPose();
77  float leftForceAmplifier = 2.0;
78  float rightForceAmplifier = 1.2;
79  float forceThreshold = in.getForceThreshhold();
80  float desiredDistance = (desiredLeftTcpPose->toEigen() - desiredRightTcpPose->toEigen()).norm();
81  float accGain = 120.0;
82  float decGain = 1.0;
83  float maxVelocity = 50.0;
84  float torqueThreshold = in.getTorqueThreshhold();
85  float oriAccGain = 4.0;
86  float oriDccGain = 1.0;
87  float maxOriAcc = 1.0;
88  float maxOriVelocity = 0.1;
89  float stiffness = 0.5 * accGain;
90  float damping = 1.0 / (2.0 * sqrt(stiffness));
91 
92  if (torqueThreshold == 0.0)
93  {
94  oriAccGain = 0.0;
95  }
96 
97  if (forceThreshold == 0.0)
98  {
99  accGain = 0.0;
100  }
101 
102  // duration = IceUtil::Time::milliSecondsDouble(0);
103 
104  Eigen::Matrix4f leftTcpPoseBase =
105  context->getRobot()->getRobotNode(in.getLeftHandName())->getPoseInRootFrame();
106  Eigen::Matrix4f rightTcpPoseBase =
107  context->getRobot()->getRobotNode(in.getRightHandName())->getPoseInRootFrame();
108 
109  Eigen::Vector3f currentLeftRPY;
110  VirtualRobot::MathTools::eigen4f2rpy(leftTcpPoseBase, currentLeftRPY);
111  Eigen::Vector3f currentRightRPY;
112  VirtualRobot::MathTools::eigen4f2rpy(rightTcpPoseBase, currentRightRPY);
113 
114 
115  Eigen::Matrix4f deltaLeftTcpPose = desiredLeftTcpPose->toEigen() * leftTcpPoseBase.inverse();
116  Eigen::Matrix4f deltaRightTcpPose = desiredRightTcpPose->toEigen() * rightTcpPoseBase.inverse();
117 
118  Eigen::Vector3f deltaLeftRPY;
119  VirtualRobot::MathTools::eigen4f2rpy(deltaLeftTcpPose, deltaLeftRPY);
120  Eigen::Vector3f deltaRightRPY;
121  VirtualRobot::MathTools::eigen4f2rpy(deltaRightTcpPose, deltaRightRPY);
122 
123  DatafieldRefPtr forceRefLeft = DatafieldRefPtr::dynamicCast(
124  context->getForceTorqueUnitObserver()->getForceDatafield(leftTcpName));
125  DatafieldRefPtr torqueRefLeft = DatafieldRefPtr::dynamicCast(
126  context->getForceTorqueUnitObserver()->getTorqueDatafield(leftTcpName));
127  DatafieldRefPtr forceRefRight = DatafieldRefPtr::dynamicCast(
128  context->getForceTorqueUnitObserver()->getForceDatafield(rightTcpName));
129  DatafieldRefPtr torqueRefRight = DatafieldRefPtr::dynamicCast(
130  context->getForceTorqueUnitObserver()->getTorqueDatafield(rightTcpName));
131  /*forceRefLeft = DatafieldRefPtr::dynamicCast(context->getForceTorqueUnitObserver()->createNulledDatafield(forceRefLeft));
132  forceRefRight = DatafieldRefPtr::dynamicCast(context->getForceTorqueUnitObserver()->createNulledDatafield(forceRefRight));
133  torqueRefLeft = DatafieldRefPtr::dynamicCast(context->getForceTorqueUnitObserver()->createNulledDatafield(torqueRefLeft));
134  torqueRefRight = DatafieldRefPtr::dynamicCast(context->getForceTorqueUnitObserver()->createNulledDatafield(torqueRefRight));*/
135  FramedDirectionPtr curForceLeft = forceRefLeft->getDataField()->get<FramedDirection>();
136  curForceLeft = FramedDirection::ChangeFrame(context->getRobot(), *curForceLeft, "Armar3_Base");
137  FramedDirectionPtr curForceRight = forceRefRight->getDataField()->get<FramedDirection>();
138  curForceRight =
139  FramedDirection::ChangeFrame(context->getRobot(), *curForceRight, "Armar3_Base");
140  FramedDirectionPtr curTorqueLeft = torqueRefLeft->getDataField()->get<FramedDirection>();
141  curTorqueLeft =
142  FramedDirection::ChangeFrame(context->getRobot(), *curTorqueLeft, "Armar3_Base");
143  FramedDirectionPtr curTorqueRight = torqueRefRight->getDataField()->get<FramedDirection>();
144  curTorqueRight =
145  FramedDirection::ChangeFrame(context->getRobot(), *curTorqueRight, "Armar3_Base");
146  FramedDirectionPtr curVelLeft = in.getCurrentLeftTcpPositionVelocity();
147  curVelLeft = FramedDirection::ChangeFrame(context->getRobot(), *curVelLeft, "Armar3_Base");
148  FramedDirectionPtr curVelRight = in.getCurrentRightTcpPositionVelocity();
149  curVelRight = FramedDirection::ChangeFrame(context->getRobot(), *curVelRight, "Armar3_Base");
150  FramedDirectionPtr curVelOriLeft = in.getCurrentLeftTcpPositionVelocity();
151  curVelOriLeft =
152  FramedDirection::ChangeFrame(context->getRobot(), *curVelOriLeft, "Armar3_Base");
153  FramedDirectionPtr curVelOriRight = in.getCurrentRightTcpPositionVelocity();
154  curVelOriRight =
155  FramedDirection::ChangeFrame(context->getRobot(), *curVelOriRight, "Armar3_Base");
156 
157  ARMARX_INFO << "Measuring forces in frame: " << curForceLeft->frame;
158  Eigen::Vector3f newVelLeft(3);
159  Eigen::Vector3f newAccLeft(3);
160  Eigen::Vector3f newVelRight(3);
161  Eigen::Vector3f newAccRight(3);
162  Eigen::Vector3f curForceRightVec = curForceRight->toEigen();
163  Eigen::Vector3f curForceLeftVec = curForceLeft->toEigen();
164  Eigen::Vector3f curTorqueRightVec = curTorqueRight->toEigen();
165  Eigen::Vector3f curTorqueLeftVec = curTorqueLeft->toEigen();
166  ARMARX_INFO << "current Force right hand " << curForceRightVec(0) << " " << curForceRightVec(1)
167  << " " << curForceRightVec(2);
168 
169  curForceRightVec = curForceRightVec * rightForceAmplifier;
170  curForceLeftVec = curForceLeftVec * leftForceAmplifier;
171 
172  ARMARX_INFO << "current Force right hand wo init " << curForceRightVec(0) << " "
173  << curForceRightVec(1) << " " << curForceRightVec(2);
174  ARMARX_INFO << "current Force left hand wo init " << curForceLeftVec(0) << " "
175  << curForceLeftVec(1) << " " << curForceLeftVec(2);
176  /*if (counter > 300)
177  {
178  curForceLeftVec(1) = 0.0;
179  curForceLeftVec(0) = 0.0;
180  curForceRightVec(1) = 0.0;
181  curForceRightVec(0) = 0.0;
182  }
183  else
184  {
185  curForceLeftVec(1) += 20.0;
186  curForceLeftVec(0) += 20.0;
187  //curForceRightVec(1) += 20.0;current Force right hand wo init 10.3537 7.50225 1.28756
188  current Force right hand wo init -3.64839 7.4992 4.22474
189  //curForceRightVec(0) += 20.0;
190  }*/
191 
192  out.setcounter(counter);
193  //Eigen::Vector3f curTorqueRightVec = curTorqueRight->toEigen();
194  //Eigen::Vector3f curTorqueLeftVec = curTorqueLeft->toEigen();
195  curForceRightVec(1) = 2 * curForceRightVec(1);
196  curForceLeftVec(1) = 2 * curForceLeftVec(1);
197 
198  curForceRightVec(2) = 0.0; //2*curForceRightVec(2);
199  curForceLeftVec(2) = 0.0; //2*curForceLeftVec(2);
200  Eigen::Vector3f leftToRightVec;
201  leftToRightVec(0) = rightTcpPoseBase(0, 3) - leftTcpPoseBase(0, 3);
202  leftToRightVec(1) = rightTcpPoseBase(1, 3) - leftTcpPoseBase(1, 3);
203  leftToRightVec(2) = rightTcpPoseBase(2, 3) - leftTcpPoseBase(2, 3);
204  Eigen::Vector3f forceLeftToRightVec;
205  forceLeftToRightVec(0) = 2.0 * forceThreshold + curForceRightVec(0) - curForceLeftVec(0);
206  forceLeftToRightVec(1) = curForceRightVec(1) - curForceLeftVec(1);
207  forceLeftToRightVec(2) = 0.0; //curForceRightVec(2)-curForceLeftVec(2);
208  Eigen::Vector3f meanForceVec = 0.5 * (curForceLeftVec + curForceRightVec);
209  Eigen::Vector3f platformAccVec;
210  Eigen::Vector3f curPlatformVel = in.getCurrentPlatformVelocity()->toEigen();
211  bool useCurrentVel = false;
212  Eigen::Vector3f meanForceVec2 = meanForceVec;
213  meanForceVec2(2) = 0;
214 
215  if (meanForceVec2.norm() <
216  forceThreshold) // || forceLeftToRightVec.norm() < 2.1*forceThreshold)
217  {
218  //if (platformAccVec.norm() > 0)
219  // newAccLeft = -decGain*curVelLeft->toEigen().normalized() + centerForceLeftVec;
220 
221  if (curPlatformVel.norm() > 0)
222  {
223  platformAccVec(0) =
224  -0.5 * curPlatformVel(0) / (duration.toMilliSecondsDouble() * 0.001);
225  platformAccVec(1) =
226  -0.5 * curPlatformVel(1) / (duration.toMilliSecondsDouble() * 0.001);
227  platformAccVec(2) =
228  -0.5 * curPlatformVel(2) / (duration.toMilliSecondsDouble() * 0.001);
229  useCurrentVel = true;
230  }
231  else
232  {
233  platformAccVec(0) = 0.0;
234  platformAccVec(1) = 0.0;
235  platformAccVec(2) = 0.0;
236  }
237 
238  if (!in.getinitialForcesReset())
239  {
240  /*forceRefLeft = DatafieldRefPtr::dynamicCast(context->getForceTorqueUnitObserver()->createNulledDatafield(forceRefLeft));
241  forceRefRight = DatafieldRefPtr::dynamicCast(context->getForceTorqueUnitObserver()->createNulledDatafield(forceRefRight));
242  torqueRefLeft = DatafieldRefPtr::dynamicCast(context->getForceTorqueUnitObserver()->createNulledDatafield(torqueRefLeft));
243  torqueRefRight = DatafieldRefPtr::dynamicCast(context->getForceTorqueUnitObserver()->createNulledDatafield(torqueRefRight));*/
244  }
245 
246  out.setinitialForcesReset(true);
247  }
248  else
249  {
250 
251  ARMARX_INFO << "forceLeftToRightVec " << forceThreshold << " " << forceLeftToRightVec(0)
252  << " " << forceLeftToRightVec(1) << " " << forceLeftToRightVec(2);
253  float xangle =
254  std::min(std::max(forceLeftToRightVec(0) /
255  std::sqrt(forceLeftToRightVec(0) * forceLeftToRightVec(0) +
256  forceLeftToRightVec(1) * forceLeftToRightVec(1)),
257  -1.f),
258  1.f);
259  float yangle =
260  std::min(std::max(forceLeftToRightVec(1) /
261  std::sqrt(forceLeftToRightVec(0) * forceLeftToRightVec(0) +
262  forceLeftToRightVec(1) * forceLeftToRightVec(1)),
263  -1.f),
264  1.f);
265  float angle = acos(xangle);
266 
267  if (acos(yangle) > 0.5 * M_PI)
268  {
269  angle = -angle;
270  }
271 
272  float forceAccGain = 1.0; //TODO make dependent on timestep
273  ARMARX_INFO << "Platform rotating around " << angle;
274  platformAccVec(0) = meanForceVec(0) * forceAccGain * accGain;
275  platformAccVec(1) = meanForceVec(1) * forceAccGain * accGain;
276  platformAccVec(2) = angle * forceAccGain * oriAccGain;
277 
278  out.setinitialForcesReset(false);
279  }
280 
281  Eigen::Vector3f platformVelVec = platformAccVec * duration.toMilliSecondsDouble() * 0.001;
282 
283  if (useCurrentVel)
284  {
285  platformVelVec += curPlatformVel;
286  }
287 
288  armarx::Vector3 platformVelocity(platformVelVec);
289 
290  float velForceGain = 1.0; //TODO make dependent on timestep
291  //
292  curForceLeftVec = curForceLeftVec - velForceGain * meanForceVec;
293  curForceRightVec = curForceRightVec - velForceGain * meanForceVec;
294  //TODO implement arm center Force here
295  //Spring like system
296 
297  stiffness = 1.5 * accGain;
298  damping = (2.0 * sqrt(stiffness));
299  Eigen::Vector3f centerForceLeftVec;
300  centerForceLeftVec(0) =
301  stiffness * ((desiredLeftTcpPose->toEigen())(0, 3) - leftTcpPoseBase(0, 3)) -
302  damping * curVelLeft->x;
303  centerForceLeftVec(1) =
304  stiffness * ((desiredLeftTcpPose->toEigen())(1, 3) - leftTcpPoseBase(1, 3)) -
305  damping * curVelLeft->y;
306  centerForceLeftVec(2) =
307  stiffness * ((desiredLeftTcpPose->toEigen())(2, 3) - leftTcpPoseBase(2, 3)) -
308  damping * curVelLeft->z;
309 
310 
311  Eigen::Vector3f centerForceRightVec;
312  centerForceRightVec(0) =
313  stiffness * ((desiredRightTcpPose->toEigen())(0, 3) - rightTcpPoseBase(0, 3)) -
314  damping * curVelRight->x; //250.0 Table length half
315  centerForceRightVec(1) =
316  stiffness * ((desiredRightTcpPose->toEigen())(1, 3) - rightTcpPoseBase(1, 3)) -
317  damping * curVelRight->y;
318  centerForceRightVec(2) =
319  stiffness * ((desiredRightTcpPose->toEigen())(2, 3) - rightTcpPoseBase(2, 3)) -
320  damping * curVelRight->z; //1100.0 Table length half
321 
322  //Rotatrotate first then move arms
323 
324  stiffness = accGain;
325  damping = (2.0 * sqrt(stiffness));
326  Eigen::Vector3f coupledForceLeftVec;
327  coupledForceLeftVec(0) = stiffness * ((leftToRightVec(0) / leftToRightVec.norm()) *
328  (leftToRightVec.norm() - desiredDistance)) -
329  damping * curVelLeft->x;
330  ; //-0.5*tableWidth-leftTcpPoseBase(0,3) - damping*curVelLeft->x;
331  coupledForceLeftVec(1) =
332  stiffness * ((leftToRightVec(0) / leftToRightVec.norm()) *
333  (leftToRightVec.norm() - desiredDistance)) -
334  damping * curVelLeft->y; //600.0-leftTcpPoseBase(1,3)- damping*curVelLeft->y;
335  coupledForceLeftVec(2) =
336  stiffness * ((leftToRightVec(0) / leftToRightVec.norm()) *
337  (leftToRightVec.norm() - desiredDistance)) -
338  damping * curVelLeft->z; //1100.0-leftTcpPoseBase(2,3)- damping*curVelLeft->z;
339 
340  Eigen::Vector3f coupledForceRightVec;
341 
342  coupledForceRightVec(0) = stiffness * ((-leftToRightVec(0) / leftToRightVec.norm()) *
343  (leftToRightVec.norm() - desiredDistance)) -
344  damping * curVelRight->x;
345  coupledForceRightVec(1) = stiffness * ((-leftToRightVec(1) / leftToRightVec.norm()) *
346  (leftToRightVec.norm() - desiredDistance)) -
347  damping * curVelRight->y;
348  ;
349  coupledForceRightVec(2) = stiffness * ((-leftToRightVec(2) / leftToRightVec.norm()) *
350  (leftToRightVec.norm() - desiredDistance)) -
351  damping * curVelRight->z;
352  ;
353  centerForceRightVec = centerForceRightVec + coupledForceRightVec;
354  centerForceLeftVec = centerForceLeftVec + coupledForceLeftVec;
355 
356  ARMARX_INFO << "desired left tcp pose " << *desiredLeftTcpPose;
357  ARMARX_INFO << "desired right tcp pose " << *desiredRightTcpPose;
358 
359  ARMARX_INFO << "Forces pulling TCP L to center " << centerForceLeftVec(0) << " "
360  << centerForceLeftVec(1) << " " << centerForceLeftVec(2);
361  ARMARX_INFO << "Forces pulling TCP R to center " << centerForceRightVec(0) << " "
362  << centerForceRightVec(1) << " " << centerForceRightVec(2);
363  ARMARX_INFO << "Forces acting on TCP L " << curForceLeftVec(0) * accGain << " "
364  << curForceLeftVec(1) * accGain << " " << curForceLeftVec(2) * accGain;
365  ARMARX_INFO << "Forces acting on TCP R " << curForceRightVec(0) * accGain << " "
366  << curForceRightVec(1) * accGain << " " << curForceRightVec(2) * accGain;
367 
368  if (curForceLeftVec.norm() > forceThreshold)
369  {
370  newAccLeft = curForceLeftVec * accGain + centerForceLeftVec;
371  }
372  else if (curVelLeft->toEigen().norm() > 0)
373  {
374  ARMARX_IMPORTANT << deactivateSpam(1) << "Deccelerating";
375  newAccLeft = -decGain * curVelLeft->toEigen().normalized() + centerForceLeftVec;
376  }
377  else
378  {
379  newAccLeft = Eigen::Vector3f::Zero() + centerForceLeftVec;
380  }
381 
382  if (curForceRightVec.norm() > forceThreshold)
383  {
384  newAccRight = curForceRightVec * accGain + centerForceRightVec;
385  }
386  else if (curVelRight->toEigen().norm() > 0)
387  {
388  ARMARX_IMPORTANT << deactivateSpam(1) << "Deccelerating";
389  newAccRight = -decGain * curVelRight->toEigen().normalized() + centerForceRightVec;
390  }
391  else
392  {
393  newAccRight = Eigen::Vector3f::Zero() + centerForceRightVec;
394  }
395 
396  //ARMARX_IMPORTANT << deactivateSpam(1) << "current curForce: " << curForce->output();
397  //ARMARX_IMPORTANT << deactivateSpam(1) << "current velocity: " << vel->output();
398  newVelLeft = curVelLeft->toEigen() + newAccLeft * duration.toMilliSecondsDouble() * 0.001;
399 
400  if (newVelLeft.norm() > maxVelocity)
401  {
402  newVelLeft = newVelLeft.normalized() * maxVelocity;
403  }
404 
405  newVelRight = curVelRight->toEigen() + newAccRight * duration.toMilliSecondsDouble() * 0.001;
406 
407  if (newVelRight.norm() > maxVelocity)
408  {
409  newVelRight = newVelRight.normalized() * maxVelocity;
410  }
411 
412 
413  Eigen::Vector3f newOriVelLeft(3);
414  Eigen::Vector3f newOriAccLeft(3);
415 
416  //ARMARX_INFO << deactivateSpam(1) << "force magnitude: " << curToruqe->toEigen().norm();
417  if (curTorqueLeftVec.norm() > torqueThreshold)
418  {
419  newOriAccLeft = curTorqueLeftVec * oriAccGain;
420  }
421  else if (curVelOriLeft->toEigen().norm() > 0)
422  {
423  //ARMARX_IMPORTANT << deactivateSpam(1) << "Deccelerating";
424  newOriAccLeft = -oriDccGain * curVelOriLeft->toEigen().normalized();
425  }
426  else
427  {
428  newOriAccLeft = Eigen::Vector3f::Zero();
429  }
430 
431  //ARMARX_IMPORTANT << deactivateSpam(1) << "current curTorque: " << curTorque->output();
432  //ARMARX_IMPORTANT << deactivateSpam(1) << "current velocity: " << velOriLeft->output();
433  Eigen::Vector3f velOriLeftDelta = newOriAccLeft * duration.toMilliSecondsDouble() * 0.001;
434 
435  if (velOriLeftDelta.norm() > maxOriAcc)
436  {
437  velOriLeftDelta = velOriLeftDelta.normalized() * maxOriAcc;
438  }
439 
440  newOriVelLeft = curVelOriLeft->toEigen() + velOriLeftDelta;
441 
442  if (newOriVelLeft.norm() > maxOriVelocity)
443  {
444  newOriVelLeft = newOriVelLeft.normalized() * maxOriVelocity;
445  }
446 
447  // ARMARX_IMPORTANT << deactivateSpam(1) << "current newVel: " << newOriVel;
448  // vel->x = 0;
449  // vel->y = 0;
450 
451 
452  //float maxSensitivity = 2.0f;
453 
454  Eigen::Vector3f newOriVelRight(3);
455  Eigen::Vector3f newOriAccRight(3);
456 
457  if (curTorqueRightVec.norm() > torqueThreshold)
458  {
459  ARMARX_INFO << deactivateSpam(1) << "zero force";
460  newOriAccRight = curTorqueRightVec * oriAccGain;
461  }
462  else if (curVelOriRight->toEigen().norm() > 0)
463  {
464  ARMARX_IMPORTANT << deactivateSpam(1) << "Deccelerating";
465  newOriAccRight = -oriDccGain * curVelOriRight->toEigen().normalized();
466  }
467  else
468  {
469  newOriAccRight = Eigen::Vector3f::Zero();
470  }
471 
472  ARMARX_INFO << "New Ori Acc Right " << newOriAccRight(0) << " " << newOriAccRight(1) << " "
473  << newOriAccRight(2);
474  //ARMARX_IMPORTANT << deactivateSpam(1) << "current curTorque: " << curTorque->output();
475  //ARMARX_IMPORTANT << deactivateSpam(1) << "current velocity: " << velOriRight->output();
476  Eigen::Vector3f velOriRightDelta = newOriAccRight * duration.toMilliSecondsDouble() * 0.001;
477 
478  if (velOriRightDelta.norm() > maxOriAcc)
479  {
480  velOriRightDelta = velOriRightDelta.normalized() * maxOriAcc;
481  }
482 
483  newOriVelRight = curVelOriRight->toEigen() + velOriRightDelta;
484 
485  if (newOriVelRight.norm() > maxOriVelocity)
486  {
487  newOriVelRight = newOriVelRight.normalized() * maxOriVelocity;
488  }
489 
490  ARMARX_INFO << "New Ori Vel Right " << newOriVelRight(0) << " " << newOriVelRight(1) << " "
491  << newOriVelRight(2);
492  //platformVelocity.x = platformGain*0.5*(newVelLeft(0)+newVelRight(0));
493  //platformVelocity.y = platformGain*0.5*(newVelLeft(1)+newVelRight(1));
494  //platformVelocity.z = platformGain*0.5*(newOriVelLeft(2)+newOriVelRight(2));
495  float maxVelocityTrans = 60.0;
496  float maxVelocityRot = 0.2;
497  //float minVelocityTrans = 30.0; //unused variable
498  //float minVelocityRot = 0.05; //unused variable
499  Vector3Ptr platformVelPtr = new Vector3(platformVelocity.toEigen());
500 
501  assert(kernelSize >= 0);
502  if (platformVelMeanFilter.size() == static_cast<std::size_t>(kernelSize))
503  {
504  platformVelMeanFilter.erase(platformVelMeanFilter.begin(),
505  platformVelMeanFilter.begin() + 1);
506  }
507 
508  platformVelMeanFilter.push_back(platformVelPtr);
509 
510  platformVelocity.x = 0.0;
511  platformVelocity.y = 0.0;
512  platformVelocity.z = 0.0;
513 
514  for (std::size_t i = 0; i < platformVelMeanFilter.size(); i++)
515  {
516  platformVelocity.x += platformVelMeanFilter[i]->x;
517  platformVelocity.y += platformVelMeanFilter[i]->y;
518  platformVelocity.z += platformVelMeanFilter[i]->z;
519  }
520 
521  platformVelocity.x /= float(platformVelMeanFilter.size());
522  platformVelocity.y /= float(platformVelMeanFilter.size());
523  platformVelocity.z /= float(platformVelMeanFilter.size());
524  platformVelocity.x =
525  std::max(std::min(platformVelocity.x, maxVelocityTrans), -maxVelocityTrans);
526  platformVelocity.y =
527  std::max(std::min(platformVelocity.y, maxVelocityTrans), -maxVelocityTrans);
528  platformVelocity.z = std::max(std::min(platformVelocity.z, maxVelocityRot), -maxVelocityRot);
529  Eigen::Matrix4f platformTransformation;
530  platformTransformation << cos(platformVelocity.z), -sin(platformVelocity.z), 0.0, 0.0,
531  sin(platformVelocity.z), cos(platformVelocity.z), 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,
532  0.0, 1.0;
533  Eigen::Vector4f temp;
534 
535  for (int i = 0; i < 3; i++)
536  {
537  temp(i) = newVelLeft(i);
538  }
539 
540  temp(3) = 1.0;
541  temp = platformTransformation.inverse() * temp;
542 
543  for (int i = 0; i < 3; i++)
544  {
545  newVelLeft(i) = temp(i);
546  }
547 
548  for (int i = 0; i < 3; i++)
549  {
550  temp(i) = newOriVelLeft(i);
551  }
552 
553  temp(3) = 1.0;
554  temp = platformTransformation.inverse() * temp;
555 
556  for (int i = 0; i < 3; i++)
557  {
558  newOriVelLeft(i) =
559  oriAccGain *
560  deltaLeftRPY(
561  i); //((in.getDesiredLeftTcpOrientation()->toEigen())(i)-currentLeftRPY(i))*10.0;//duration.toMilliSecondsDouble()*0.001;//temp(i);
562  }
563 
564  for (int i = 0; i < 3; i++)
565  {
566  temp(i) = newVelRight(i);
567  }
568 
569  temp(3) = 1.0;
570  temp = platformTransformation.inverse() * temp;
571 
572  for (int i = 0; i < 3; i++)
573  {
574  newVelRight(i) = temp(i);
575  }
576 
577  for (int i = 0; i < 3; i++)
578  {
579  temp(i) = newOriVelRight(i);
580  }
581 
582  temp(3) = 1.0;
583  temp = platformTransformation.inverse() * temp;
584 
585  for (int i = 0; i < 3; i++)
586  {
587  newOriVelRight(i) =
588  oriAccGain *
589  deltaRightRPY(
590  i); //((in.getDesiredRightTcpOrientation()->toEigen())(i)-currentRightRPY(i))*10.0;//duration.toMilliSecondsDouble()*0.001;//temp(i);
591  }
592 
593  ARMARX_INFO << "New Vel right pos " << newVelRight(0) << " " << newVelRight(1) << " "
594  << newVelRight(2);
595  ARMARX_INFO << "New Vel right ori " << newOriVelRight(0) << " " << newOriVelRight(1) << " "
596  << newOriVelRight(2);
597  ARMARX_INFO << "New Vel right pos " << newVelRight(0) << " " << newVelRight(1) << " "
598  << newVelRight(2);
599  ARMARX_INFO << "New Vel left ori " << newOriVelLeft(0) << " " << newOriVelLeft(1) << " "
600  << newOriVelLeft(2);
601  Eigen::Matrix4f targetLeft;
602  VirtualRobot::MathTools::rpy2eigen4f(in.getDesiredLeftTcpOrientation()->toEigen()(0),
603  in.getDesiredLeftTcpOrientation()->toEigen()(1),
604  in.getDesiredLeftTcpOrientation()->toEigen()(2),
605  targetLeft);
606  targetLeft(0, 3) = leftTcpPoseBase(0, 3);
607  targetLeft(1, 3) = leftTcpPoseBase(1, 3);
608  targetLeft(2, 3) = leftTcpPoseBase(2, 3);
609  Eigen::Matrix4f targetright;
610  VirtualRobot::MathTools::rpy2eigen4f(in.getDesiredRightTcpOrientation()->toEigen()(0),
611  in.getDesiredRightTcpOrientation()->toEigen()(1),
612  in.getDesiredRightTcpOrientation()->toEigen()(2),
613  targetright);
614  targetright(0, 3) = rightTcpPoseBase(0, 3);
615  targetright(1, 3) = rightTcpPoseBase(1, 3);
616  targetright(2, 3) = rightTcpPoseBase(2, 3);
617  ARMARX_INFO << "target RPY right " << currentRightRPY(0) << " " << currentRightRPY(1) << " "
618  << currentRightRPY(2);
619  ARMARX_INFO << "target RPY left " << currentLeftRPY(0) << " " << currentLeftRPY(1) << " "
620  << currentLeftRPY(2);
621  ARMARX_INFO << "target RPY right " << (in.getDesiredRightTcpOrientation()->toEigen())(0) << " "
622  << (in.getDesiredRightTcpOrientation()->toEigen())(1) << " "
623  << (in.getDesiredRightTcpOrientation()->toEigen())(2);
624  ARMARX_INFO << "target RPY left " << (in.getDesiredLeftTcpOrientation()->toEigen())(0) << " "
625  << (in.getDesiredLeftTcpOrientation()->toEigen())(1) << " "
626  << (in.getDesiredLeftTcpOrientation()->toEigen())(2);
627  auto debugDrawer = context->getDebugDrawerTopicProxy();
628  debugDrawer->setPoseVisu(
629  "leftHandTCP",
630  "leftHandTCP",
631  FramedPose(leftTcpPoseBase, "Armar3_Base", "Armar3").toGlobal(context->getRobot()));
632  debugDrawer->setPoseVisu(
633  "leftTargetTCP",
634  "leftTargetTCP",
635  FramedPose(targetLeft, "Armar3_Base", "Armar3").toGlobal(context->getRobot()));
636  debugDrawer->setPoseVisu(
637  "rightHandTCP",
638  "rightHandTCP",
639  FramedPose(rightTcpPoseBase, "Armar3_Base", "Armar3").toGlobal(context->getRobot()));
640  debugDrawer->setPoseVisu(
641  "rightTargetTCP",
642  "rightTargetTCP",
643  FramedPose(targetright, "Armar3_Base", "Armar3").toGlobal(context->getRobot()));
644  //*********************Output**********************
645  out.setTimestamp(new TimestampVariant(now));
646  //out.setcurrentSensitivity(in.getcurrentSensitivity());
647 
648  out.setLeftTcpTargetPosVelocity(
649  new FramedDirection(newVelLeft, curVelLeft->frame, curVelLeft->agent));
650  out.setLeftTcpTargetOriVelocity(new FramedDirection(newOriVelLeft, "Armar3_Base", "Armar3"));
651  out.setRightTcpTargetPosVelocity(
652  new FramedDirection(newVelRight, curVelRight->frame, curVelRight->agent));
653  out.setRightTcpTargetOriVelocity(new FramedDirection(newOriVelRight, "Armar3_Base", "Armar3"));
654  //TODO make platform vel smaller
655  ARMARX_INFO << "Computing Platform with " << platformVelocity.x << " " << platformVelocity.y
656  << " " << platformVelocity.z;
657  out.setPlatformVelocity(platformVelocity);
658  local.setPlatformVelMeanFilter(platformVelMeanFilter);
659  //FramedDirectionPtr newFramedVel = new FramedDirection(newVel, vel->frame,vel->agent);//
660  //FramedDirectionPtr newFramedRotVel = new FramedDirection(newRotVel, velRot->frame,velRot->agent);
661 
662 
663  /*newFramedVel = FramedDirection::ChangeFrame(c->getRobot(), *newFramedVel, c->getRobot()->getRootNode()->getName());
664  ARMARX_IMPORTANT << deactivateSpam(1) << "current newVel in platform: " << newFramedVel->output();
665  newFramedRotVel = FramedDirection::ChangeFrame(c->getRobot(), *newFramedRotVel, c->getRobot()->getRootNode()->getName());
666  ARMARX_IMPORTANT << deactivateSpam(1) << "current newVel in platform: " << newFramedRotVel->output();*/
667  bool noChange = false;
668 
669  if (noChange)
670  {
671  //TODO set to zero all velocities
672  emitEvNoChange();
673  }
674  else
675  {
676  emitEvDone();
677  }
678 }
679 
680 void
681 CalculateForceControlRobotPose::onExit()
682 {
683  /*CoupledInteractionGroupStatechartContext* context =*/getContext<
684  CoupledInteractionGroupStatechartContext>(); //unused variable
685  /*std::string leftTcpName("Wrist 2 L");
686  std::string rightTcpName("Wrist 2 R");
687  DatafieldRefPtr forceRefLeft = DatafieldRefPtr::dynamicCast(context->getForceTorqueUnitObserver()->getForceDatafield(leftTcpName));
688  DatafieldRefPtr torqueRefLeft = DatafieldRefPtr::dynamicCast(context->getForceTorqueUnitObserver()->getTorqueDatafield(leftTcpName));
689  DatafieldRefPtr forceRefRight = DatafieldRefPtr::dynamicCast(context->getForceTorqueUnitObserver()->getForceDatafield(rightTcpName));
690  DatafieldRefPtr torqueRefRight = DatafieldRefPtr::dynamicCast(context->getForceTorqueUnitObserver()->getTorqueDatafield(rightTcpName));
691  context->getForceTorqueUnitObserver()->removeFilteredDatafield(forceRefLeft);
692  context->getForceTorqueUnitObserver()->removeFilteredDatafield(forceRefRight);
693  context->getForceTorqueUnitObserver()->removeFilteredDatafield(torqueRefLeft);
694  context->getForceTorqueUnitObserver()->removeFilteredDatafield(torqueRefRight);*/
695  // put your user code for the exit point here
696  // execution time should be short (<100ms)
697 }
698 
699 // DO NOT EDIT NEXT FUNCTION
700 std::string
701 CalculateForceControlRobotPose::GetName()
702 {
703  return "CalculateForceControlRobotPose";
704 }
705 
706 // DO NOT EDIT NEXT FUNCTION
707 XMLStateFactoryBasePtr
708 CalculateForceControlRobotPose::CreateInstance(XMLStateConstructorParams stateData)
709 {
710  return XMLStateFactoryBasePtr(new CalculateForceControlRobotPose(stateData));
711 }
armarx::channels::PlatformUnitObserver::platformVelocity
const PlatformUnitDatafieldCreator platformVelocity("platformVelocity")
RemoteRobot.h
armarx::CoupledInteractionGroup::CalculateForceControlRobotPose::onEnter
void onEnter() override
Definition: CalculateForceControlRobotPose.cpp:49
armarx::CoupledInteractionGroup::CalculateForceControlRobotPose
Definition: CalculateForceControlRobotPose.h:37
ARMARX_IMPORTANT
#define ARMARX_IMPORTANT
Definition: Logging.h:190
armarx::VariantType::FramedPose
const VariantTypeId FramedPose
Definition: FramedPose.h:36
GfxTL::Matrix4f
MatrixXX< 4, 4, float > Matrix4f
Definition: MatrixXX.h:650
armarx::CoupledInteractionGroup::CalculateForceControlRobotPose::CreateInstance
static XMLStateFactoryBasePtr CreateInstance(XMLStateConstructorParams stateData)
Definition: CalculateForceControlRobotPose.cpp:708
armarx::CoupledInteractionGroup::CoupledInteractionGroupStatechartContext::getDebugDrawerTopicProxy
armarx::DebugDrawerInterfacePrx getDebugDrawerTopicProxy()
Definition: CoupledInteractionGroupStatechartContext.h:199
armarx::CoupledInteractionGroup::CoupledInteractionGroupStatechartContext::getForceTorqueUnitObserver
ForceTorqueUnitObserverInterfacePrx getForceTorqueUnitObserver()
Definition: CoupledInteractionGroupStatechartContext.h:221
armarx::XMLStateConstructorParams
Definition: XMLState.h:49
armarx::TimestampVariant
Definition: TimestampVariant.h:54
armarx::XMLStateFactoryBasePtr
IceInternal::Handle< XMLStateFactoryBase > XMLStateFactoryBasePtr
Definition: XMLState.h:64
IceInternal::Handle< FramedPose >
armarx::CoupledInteractionGroup::CoupledInteractionGroupStatechartContext::getRobot
const VirtualRobot::RobotPtr getRobot()
Definition: CoupledInteractionGroupStatechartContext.h:133
armarx::CoupledInteractionGroup::CalculateForceControlRobotPose::CalculateForceControlRobotPose
CalculateForceControlRobotPose(XMLStateConstructorParams stateData)
Definition: CalculateForceControlRobotPose.cpp:41
deactivateSpam
SpamFilterDataPtr deactivateSpam(SpamFilterDataPtr const &spamFilter, float deactivationDurationSec, const std::string &identifier, bool deactivate)
Definition: Logging.cpp:75
armarx::FramedDirection::ChangeFrame
static FramedDirectionPtr ChangeFrame(const VirtualRobot::RobotPtr &robot, const FramedDirection &framedVec, const std::string &newFrame)
Definition: FramedPose.cpp:91
armarx::XMLStateTemplate
Class for legacy to stay compatible with old statecharts.
Definition: XMLState.h:146
M_PI
#define M_PI
Definition: MathTools.h:17
CalculateForceControlRobotPose.h
armarx::Vector3
The Vector3 class.
Definition: Pose.h:112
max
T max(T t1, T t2)
Definition: gdiam.h:51
armarx::CoupledInteractionGroup::CalculateForceControlRobotPose::GetName
static std::string GetName()
Definition: CalculateForceControlRobotPose.cpp:701
armarx::armem::Time
armarx::core::time::DateTime Time
Definition: forward_declarations.h:13
armarx::TimeUtil::GetTime
static IceUtil::Time GetTime(TimeMode timeMode=TimeMode::VirtualTime)
Get the current time.
Definition: TimeUtil.cpp:42
GfxTL::sqrt
VectorXD< D, T > sqrt(const VectorXD< D, T > &a)
Definition: VectorXD.h:704
armarx::CoupledInteractionGroup::CalculateForceControlRobotPose::Registry
static SubClassRegistry Registry
Definition: CalculateForceControlRobotPose.h:51
TimeUtil.h
armarx::FramedDirection
FramedDirection is a 3 dimensional direction vector with a reference frame. The reference frame can b...
Definition: FramedPose.h:86
float
#define float
Definition: 16_Level.h:22
ARMARX_INFO
#define ARMARX_INFO
Definition: Logging.h:181
armarx::VariantType::FramedDirection
const VariantTypeId FramedDirection
Definition: FramedPose.h:37
armarx::Vector3::toEigen
virtual Eigen::Vector3f toEigen() const
Definition: Pose.cpp:134
memoryx::KBM::Vector3
Eigen::Vector3d Vector3
Definition: kbm.h:43
armarx::CoupledInteractionGroup::CoupledInteractionGroupStatechartContext
Definition: CoupledInteractionGroupStatechartContext.h:118
angle
double angle(const Point &a, const Point &b, const Point &c)
Definition: point.hpp:109
armarx::CoupledInteractionGroup::CalculateForceControlRobotPose::onExit
void onExit() override
Definition: CalculateForceControlRobotPose.cpp:681
min
T min(T t1, T t2)
Definition: gdiam.h:44
armarx
This file offers overloads of toIce() and fromIce() functions for STL container types.
Definition: ArmarXTimeserver.cpp:27
norm
double norm(const Point &a)
Definition: point.hpp:102