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
33using namespace armarx;
34using namespace CoupledInteractionGroup;
35
36// DO NOT EDIT NEXT LINE
37CalculateForceControlRobotPose::SubClassRegistry
38 CalculateForceControlRobotPose::Registry(CalculateForceControlRobotPose::GetName(),
39 &CalculateForceControlRobotPose::CreateInstance);
40
47
48void
49CalculateForceControlRobotPose::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
680void
681CalculateForceControlRobotPose::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
700std::string
701CalculateForceControlRobotPose::GetName()
702{
703 return "CalculateForceControlRobotPose";
704}
705
706// DO NOT EDIT NEXT FUNCTION
707XMLStateFactoryBasePtr
float
#define float
Definition 16_Level.h:22
deactivateSpam
SpamFilterDataPtr deactivateSpam(SpamFilterDataPtr const &spamFilter, float deactivationDurationSec, const std::string &identifier, bool deactivate)
Definition Logging.cpp:75
M_PI
#define M_PI
Definition MathTools.h:17
armarx::CoupledInteractionGroup::CalculateForceControlRobotPose::CreateInstance
static XMLStateFactoryBasePtr CreateInstance(XMLStateConstructorParams stateData)
Definition CalculateForceControlRobotPose.cpp:708
armarx::FramedDirection
FramedDirection is a 3 dimensional direction vector with a reference frame.
Definition FramedPose.h:87
armarx::FramedDirection::ChangeFrame
static FramedDirectionPtr ChangeFrame(const VirtualRobot::RobotConstPtr &robot, const FramedDirection &framedVec, const std::string &newFrame)
Definition FramedPose.cpp:91
armarx::FramedPose
The FramedPose class.
Definition FramedPose.h:281
armarx::TimeUtil::GetTime
static IceUtil::Time GetTime(TimeMode timeMode=TimeMode::VirtualTime)
Get the current time.
Definition TimeUtil.cpp:42
armarx::TimestampVariant
Implements a Variant type for timestamps.
Definition TimestampVariant.h:55
armarx::Vector3
The Vector3 class.
Definition Pose.h:113
armarx::XMLStateTemplate
Class for legacy to stay compatible with old statecharts.
Definition XMLState.h:147
ARMARX_INFO
#define ARMARX_INFO
The normal logging level.
Definition Logging.h:181
ARMARX_IMPORTANT
#define ARMARX_IMPORTANT
The logging level for always important information, but expected behaviour (in contrast to ARMARX_WAR...
Definition Logging.h:190
armarx
This file offers overloads of toIce() and fromIce() functions for STL container types.
Definition ArmarXTimeserver.cpp:28
armarx::Vector3Ptr
IceInternal::Handle< Vector3 > Vector3Ptr
Definition Pose.h:165
armarx::FramedDirectionPtr
IceInternal::Handle< FramedDirection > FramedDirectionPtr
Definition FramedPose.h:84
armarx::DatafieldRefPtr
IceInternal::Handle< DatafieldRef > DatafieldRefPtr
Definition Observer.h:43
armarx::XMLStateFactoryBasePtr
IceInternal::Handle< XMLStateFactoryBase > XMLStateFactoryBasePtr
Definition XMLState.h:64
armarx::toEigen
Eigen::Vector3f toEigen(const pcl::PointXYZ &pt)
Definition PointToModelICP.h:67
armarx::FramedPosePtr
IceInternal::Handle< FramedPose > FramedPosePtr
Definition FramedPose.h:272
angle
double angle(const Point &a, const Point &b, const Point &c)
Definition point.hpp:109