SimpleHandoverObject.cpp
Go to the documentation of this file.
1
#include "
SimpleHandoverObject.h
"
2
3
#include <VirtualRobot/ManipulationObject.h>
4
#include <VirtualRobot/Grasping/GraspSet.h>
5
#include <VirtualRobot/RobotNodeSet.h>
6
7
#include <
RobotAPI/libraries/core/FramedPose.h
>
8
9
namespace
armarx::skills
10
{
11
const
SkillDescription
GraspObjectSkillDesc
=
SkillDescription
{
12
"SimpleHandoverObject"
,
"Handover an object already in hand of robot"
,
13
{},
armarx::Duration::MilliSeconds
(40000),
14
grasp_object::arondto::PutdownObjectAcceptedType::ToAronType()
15
};
16
17
/*SimpleHandoverObject::SimpleHandoverObject(armem::client::MemoryNameSystem& mns, armarx::viz::Client& arviz, GraspControlSkillContext& context) :
18
Base(GraspObjectDesc, mns, arviz, context),
19
movePlatformToPose(mns, arviz, context.platformControlSkillContext)
20
{
21
}
22
23
void SimpleHandoverObject::reset()
24
{
25
Base::reset();
26
movePlatformToPose.reset();
27
}
28
29
void SimpleHandoverObject::notifyStopped()
30
{
31
Base::notifyStopped();
32
movePlatformToPose.notifyStopped();
33
}
34
35
void SimpleHandoverObject::notifyTimeoutReached()
36
{
37
Base::notifyTimeoutReached();
38
movePlatformToPose.notifyTimeoutReached();
39
}
40
41
Skill::Status SimpleHandoverObject::execute(const grasp_object::arondto::PutdownObjectAcceptedType& in, const CallbackT& callback)
42
{
43
// Members
44
armem::robot_state::VirtualRobotReader robotReader(mns);
45
armem::obj::instance::Reader objectReader(mns);
46
armem::grasping::known_grasps::Reader graspReader(mns);
47
armem::obj::instance::Writer objectWriter(mns);
48
49
robotReader.connect();
50
objectReader.connect();
51
graspReader.connect();
52
53
objectWriter.connect();
54
55
// //////////////////////////////
56
// check args
57
// //////////////////////////////
58
auto split = simox::alg::split(in.objectEntityId, "/");
59
auto objectId = in.objectEntityId;
60
if (split.size() > 3)
61
{
62
ARMARX_ERROR << "Unknown structure of object entitiy id!";
63
return Skill::Status::Failed;
64
}
65
else if (split.size() == 3)
66
{
67
objectId = split[0] + "/" + split[1];
68
}
69
70
if (in.kinematicChainName.empty())
71
{
72
ARMARX_ERROR << "No kinematic chain candidate set";
73
return Skill::Status::Failed;
74
}
75
76
auto now = armem::Time::now();
77
// //////////////////////////////
78
// get robot
79
// //////////////////////////////
80
auto robot = robotReader.getSynchronizedRobot(in.robot, now, VirtualRobot::RobotIO::RobotDescription::eStructure);
81
82
auto kinematicChain = robot->getRobotNodeSet(in.kinematicChainName);
83
const auto tcpName = kinematicChain->getTCP()->getName();
84
const auto kinematicChainJointNames = kinematicChain->getNodeNames();
85
86
87
// //////////////////////////////
88
// get object pose
89
// //////////////////////////////
90
auto objInstanceOpt = objectReader.queryObjectByEntityID(in.objectEntityId, now);
91
if (!objInstanceOpt)
92
{
93
ARMARX_ERROR << "Lost object pose.";
94
return Skill::Status::Failed;
95
}
96
auto objInstance = *objInstanceOpt;
97
98
// //////////////////////////////
99
// Setup vars
100
// //////////////////////////////
101
auto skillRet = Skill::Status::Succeeded;
102
103
auto handRootNodeName = "Hand L Base";
104
if (simox::alg::starts_with(in.kinematicChainName, "Right"))
105
{
106
handRootNodeName = "Hand R Base";
107
}
108
const FramedPosePtr handRoot = new FramedPose(robot->getRobotNode(handRootNodeName)->getPoseInRootFrame(), robot->getRootNode()->getName(), robot->getName());
109
const FramedPosePtr tcp = new FramedPose(robot->getRobotNode(tcpName)->getPoseInRootFrame(), robot->getRootNode()->getName(), robot->getName());
110
const Eigen::Matrix4f handRootPoseGlobal = handRoot->toGlobalEigen(robot);
111
const Eigen::Matrix4f tcpPoseGlobal = tcp->toGlobalEigen(robot);
112
113
const Eigen::Matrix4f tcp2handRoot = tcpPoseGlobal.inverse() * handRootPoseGlobal;
114
115
116
// //////////////////////////////
117
// Move arm back
118
// //////////////////////////////
119
{
120
auto otherArmKinematicChain = robot->getRobotNodeSet(in.otherArmKinematicChainName);
121
auto otherArmJointNames = otherArmKinematicChain->getNodeNames();
122
123
ARMARX_CHECK_EQUAL(otherArmJointNames.size(), 7); // TODO
124
125
joint_control::arondto::MoveJointsToPositionAcceptedType nextArgs;
126
nextArgs.accelerationTime = 500;
127
nextArgs.jointMaxSpeed = 0.75;
128
nextArgs.jointTargetTolerance = 0.03;
129
nextArgs.setVelocitiesToZeroAtEnd = true;
130
nextArgs.targetJointMap[otherArmJointNames[0]] = 0.47;
131
nextArgs.targetJointMap[otherArmJointNames[1]] = 0.0;
132
nextArgs.targetJointMap[otherArmJointNames[2]] = 0.0;
133
nextArgs.targetJointMap[otherArmJointNames[3]] = 0.32;
134
nextArgs.targetJointMap[otherArmJointNames[4]] = 0.47;
135
skillRet = moveJointsToPosition._execute(nextArgs, callback);
136
}
137
138
if (skillRet != Skill::Status::Succeeded)
139
{
140
clearLayer();
141
return skillRet;
142
}
143
144
// //////////////////////////////
145
// Move platform relative
146
// //////////////////////////////
147
{
148
// not used here
149
}
150
151
if (skillRet != Skill::Status::Succeeded)
152
{
153
clearLayer();
154
return skillRet;
155
}
156
157
158
// //////////////////////////////
159
// Move TCP to handover pose
160
// //////////////////////////////
161
{
162
now = IceUtil::Time::now();
163
robotReader.synchronizeRobot(*robot, now);
164
165
FramedPose tcp(robot->getRobotNode(tcpName)->getPoseInRootFrame(), robot->getRootNode()->getName(), robot->getName());
166
//const Eigen::Matrix4f robotRoot = robotRootFramedPose.toEigen();
167
//const Eigen::Matrix4f robotRootPoseGlobalEigen = robotRootFramedPose.toGlobalEigen(robot);
168
169
// query obj target location from grasp
170
auto objPoseRobot = objInstance.pose.objectPoseRobot;
171
objPoseRobot(1,3) += 40; // move further away
172
FramedPose targetObjectFramedPoseRoot(objPoseRobot, robot->getRootNode()->getName(), robot->getName());
173
const auto targetObjectPoseGlobalEigen = targetObjectFramedPoseRoot.toGlobalEigen(robot);
174
175
const auto transformationFromTCPToObject = objInstance.pose.attachment.poseInFrame;
176
177
const auto placePoseGlobal = Eigen::Matrix4f(targetObjectPoseGlobalEigen * transformationFromTCPToObject.inverse());
178
179
Eigen::Matrix4f handRootTargetPoseGlobal = placePoseGlobal * tcp2handRoot;
180
handRootTargetPoseGlobal(2,3) += 20;
181
182
auto l = arviz.layer(layerName);
183
auto o = armarx::viz::Robot("grasp");
184
o.file("RobotAPI", "RobotAPI/robots/Armar3/ArmarIII-LeftHand.xml"); // TODO!
185
o.pose(handRootTargetPoseGlobal);
186
l.add(o);
187
arviz.commit(l);
188
189
{
190
tcp_control::arondto::MoveTCPToTargetPoseAcceptedType nextArgs;
191
nextArgs.kinematicChainName = in.kinematicChainName;
192
nextArgs.orientationalAccuracy = in.orientationalAccuracy;
193
nextArgs.positionalAccuracy = in.positionalAccuracy;
194
nextArgs.targetPoseGlobal = handRootTargetPoseGlobal;
195
skillRet = moveTcpToTargetSkill.execute(nextArgs, callback);
196
}
197
198
clearLayer();
199
}
200
201
202
if (skillRet != Skill::Status::Succeeded)
203
{
204
clearLayer();
205
return skillRet;
206
}
207
208
209
// //////////////////////////////
210
// Open hand and detach from memory
211
// //////////////////////////////
212
{
213
now = IceUtil::Time::now();
214
robotReader.synchronizeRobot(*robot, now);
215
216
FramedPose robotRootFramedPose(robot->getRootNode()->getPoseInRootFrame(), robot->getRootNode()->getName(), robot->getName());
217
//const Eigen::Matrix4f robotRoot = robotRootFramedPose.toEigen();
218
const Eigen::Matrix4f robotRootGlobal = robotRootFramedPose.toGlobalEigen(robot);
219
220
FramedPose objInRobotRootFramedPose(objInstance.pose.objectPoseRobot, robot->getRootNode()->getName(), robot->getName());
221
const auto objPoseGlobalEigen = objInRobotRootFramedPose.toGlobalEigen(robot);
222
const auto objPoseRootEigen = objInRobotRootFramedPose.toEigen();
223
224
//objInstanceUpdate.pose.providerName = description.skillName;
225
objInstance.pose.attachmentValid = false;
226
objInstance.pose.attachment.resetHard();
227
objInstance.pose.objectPoseGlobal = objPoseGlobalEigen;
228
objInstance.pose.objectPoseRobot = objPoseRootEigen;
229
objInstance.pose.robotPose = robotRootGlobal;
230
objInstance.pose.robotConfig = robot->getJointValues();
231
232
objectWriter.commitObject(objInstance, objInstance.pose.providerName, IceUtil::Time::now());
233
234
skillRet = openHand.execute(nullptr, callback);
235
}
236
237
if (skillRet != Skill::Status::Succeeded)
238
{
239
clearLayer();
240
return skillRet;
241
}
242
243
244
// //////////////////////////////
245
// Move TCP to retreat pose
246
// //////////////////////////////
247
{
248
const float xRetreatOffset = 90;
249
const float yRetreatOffset = 10;
250
251
Eigen::Matrix4f offset = Eigen::Matrix4f::Identity();
252
offset(0,3) = xRetreatOffset;
253
offset(1,3) = yRetreatOffset;
254
255
now = IceUtil::Time::now();
256
robotReader.synchronizeRobot(*robot, now);
257
258
auto TcpPoseGlobalEigen = robot->getRobotNode(tcpName)->getGlobalPose();
259
Eigen::Matrix4f newTCPPoseGlobalEigen = TcpPoseGlobalEigen * offset;
260
261
tcp_control::arondto::MoveTCPToTargetPoseAcceptedType nextArgs;
262
nextArgs.kinematicChainName = in.kinematicChainName;
263
nextArgs.orientationalAccuracy = in.orientationalAccuracy;
264
nextArgs.positionalAccuracy = in.positionalAccuracy;
265
nextArgs.targetPoseGlobal = newTCPPoseGlobalEigen;
266
skillRet = moveTcpToTargetSkill.execute(nextArgs, callback);
267
}
268
269
if (skillRet != Skill::Status::Succeeded)
270
{
271
clearLayer();
272
return skillRet;
273
}
274
275
// //////////////////////////////
276
// Move joints to zero position
277
// //////////////////////////////
278
{
279
joint_control::arondto::MoveJointsToPositionAcceptedType nextArgs;
280
nextArgs.accelerationTime = 500;
281
nextArgs.jointMaxSpeed = 0.75;
282
nextArgs.jointTargetTolerance = 0.03;
283
nextArgs.setVelocitiesToZeroAtEnd = true;
284
for (const auto& j : kinematicChainJointNames)
285
{
286
nextArgs.targetJointMap[j] = 0.0;
287
}
288
for (const auto& j : robot->getRobotNodeSet(in.otherArmKinematicChainName)->getNodeNames())
289
{
290
nextArgs.targetJointMap[j] = 0.0;
291
}
292
skillRet = moveJointsToPosition._execute(nextArgs, callback);
293
}
294
295
clearLayer();
296
return skillRet;
297
}*/
298
}
armarx::skills
This file is part of ArmarX.
Definition:
PeriodicUpdateWidget.cpp:11
armarx::skills::SkillDescription
Definition:
SkillDescription.h:18
SimpleHandoverObject.h
FramedPose.h
armarx::skills::GraspObjectSkillDesc
const SkillDescription GraspObjectSkillDesc
Definition:
SimpleHandoverObject.cpp:11
armarx::core::time::Duration::MilliSeconds
static Duration MilliSeconds(std::int64_t milliSeconds)
Constructs a duration in milliseconds.
Definition:
Duration.cpp:55
RobotSkillTemplates
libraries
skill_grasp_object
handover
SimpleHandoverObject.cpp
Generated on Sat Oct 5 2024 09:14:05 for armarx_documentation by
1.8.17