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