Added tcp pose to grasp candidate and bimanual graspcandidate (ice and armem)

source/RobotAPI/components/armem/client/GraspProviderExample/GraspProviderExample.cpp

@@ -90,7 +90,7 @@ namespace armarx
             candidate.graspPose = PoseBasePtr(toIce(Eigen::Matrix4f()));
             candidate.providerName = "Example";
             candidate.robotPose = PoseBasePtr(toIce(Eigen::Matrix4f()));
-
+            candidate.tcpPoseInHandRoot = PoseBasePtr(toIce(Eigen::Matrix4f()));
             // source Info is also not necessary, but reference object name is used as entity name
             // "UnknownObject" if none is provided
             candidate.sourceInfo = new grasping::GraspCandidateSourceInfo();
@@ -112,6 +112,8 @@ namespace armarx
             bimanualCandidate.graspPoseRight = PoseBasePtr(toIce(Eigen::Matrix4f()));
             bimanualCandidate.providerName = "BimanualExample";
             bimanualCandidate.robotPose = PoseBasePtr(toIce(Eigen::Matrix4f()));
+            bimanualCandidate.tcpPoseInHandRootRight = PoseBasePtr(toIce(Eigen::Matrix4f()));
+            bimanualCandidate.tcpPoseInHandRootLeft = PoseBasePtr(toIce(Eigen::Matrix4f()));
             bimanualCandidate.inwardsVectorLeft = Vector3BasePtr(toIce(Eigen::Vector3f()));
             bimanualCandidate.inwardsVectorRight = Vector3BasePtr(toIce(Eigen::Vector3f()));
 

source/RobotAPI/interface/units/GraspCandidateProviderInterface.ice

@@ -82,8 +82,8 @@ module armarx
         {
             PoseBase graspPose;
             PoseBase robotPose;
-            Vector3Base approachVector;
             PoseBase tcpPoseInHandRoot;
+            Vector3Base approachVector;
 
             string sourceFrame;  // frame where graspPose is located
             string targetFrame;  // frame which should be moved to graspPose
@@ -105,6 +105,8 @@ module armarx
             PoseBase graspPoseRight;
             PoseBase graspPoseLeft;
             PoseBase robotPose;
+            PoseBase tcpPoseInHandRootRight;
+            PoseBase tcpPoseInHandRootLeft;
 
             Vector3Base approachVectorRight;
             Vector3Base approachVectorLeft;

source/RobotAPI/libraries/GraspingUtility/aron/GraspCandidate.xml

@@ -95,6 +95,10 @@
                 <Pose />
             </ObjectChild>
 
+            <ObjectChild key='tcpPoseInHandRoot'>
+                <Pose />
+            </ObjectChild>
+
             <ObjectChild key='approachVector'>
                 <Position />
             </ObjectChild>
@@ -164,6 +168,14 @@
                 <Pose />
             </ObjectChild>
 
+            <ObjectChild key='tcpPoseInHandRootRight'>
+                <Pose />
+            </ObjectChild>
+
+            <ObjectChild key='tcpPoseInHandRootLeft'>
+                <Pose />
+            </ObjectChild>
+
             <ObjectChild key='approachVectorRight'>
                 <Position />
             </ObjectChild>

source/RobotAPI/libraries/GraspingUtility/aron_conversions.cpp

@@ -66,6 +66,7 @@ void armarx::grasping::fromAron(const armarx::grasping::arondto::GraspCandidate&
     bo = GraspCandidate();
     bo.graspPose = toIce(dto.graspPose);
     bo.robotPose = toIce(dto.robotPose);
+    bo.tcpPoseInHandRoot = toIce(dto.tcpPoseInHandRoot);
     bo.approachVector = toIce(dto.approachVector);
     bo.sourceFrame = dto.sourceFrame;
     bo.targetFrame = dto.targetFrame;
@@ -133,6 +134,7 @@ void armarx::grasping::toAron(armarx::grasping::arondto::GraspCandidate& dto,
         dto.reachabilityInfo.toAron();
     }
     dto.robotPose = fromIce(bo.robotPose);
+    dto.tcpPoseInHandRoot = fromIce(bo.tcpPoseInHandRoot);
     dto.side = bo.side;
     dto.sourceFrame = bo.sourceFrame;
     if (bo.sourceInfo)
@@ -155,6 +157,8 @@ void armarx::grasping::fromAron(const armarx::grasping::arondto::BimanualGraspCa
     bo.graspPoseRight = toIce(dto.graspPoseRight);
     bo.graspPoseLeft = toIce(dto.graspPoseLeft);
     bo.robotPose = toIce(dto.robotPose);
+    bo.tcpPoseInHandRootRight = toIce(dto.tcpPoseInHandRootRight);
+    bo.tcpPoseInHandRootLeft = toIce(dto.tcpPoseInHandRootLeft);
     bo.approachVectorRight = toIce(dto.approachVectorRight);
     bo.approachVectorLeft = toIce(dto.approachVectorLeft);
     bo.inwardsVectorRight = toIce(dto.inwardsVectorRight);