patch: get object orientation in iCub simulator

classic Classic list List threaded Threaded
2 messages Options
Reply | Threaded
Open this post in threaded view
|

patch: get object orientation in iCub simulator

Fabien Benureau
This post was updated on .
Hi, I wrote a bit of code to get the object orientation in the iCub simulator (previously, you could only set it).
I am posting it here for review. It's pretty straightforward, and involve copy/past/adapt of code for getting position, with the extra hurdle that ode gives you a rotation matrix instead of a vector of angles.

As-is, it introduces the following syntax to get object orientation :
world rot <object-kind> <object-id>

Example usage :
$ yarp rpc /icubSim/world
world mk box 0.2 0.2 0.2 0.3 0.2 1 1 0 0
Response: [ok]
world rot box 1
Response: 0.0 -0.0 0.0
world rot box 1 0 45 0
Response: [ok]
world rot box 1
Response: -0.0 45.0 0.0
world rot box 1 90 0 0
Response: [ok]
world rot box 1
Response: 90.0 0.0 0.0

Patch :
Index: src/simulators/iCubSimulation/wrapper/WorldManager.cpp
===================================================================
--- src/simulators/iCubSimulation/wrapper/WorldManager.cpp	(revision 19662)
+++ src/simulators/iCubSimulation/wrapper/WorldManager.cpp	(working copy)
@@ -343,7 +343,9 @@
 
 bool doRotate(ManagerState& state) {
     consumeObject(state);
-    state.consume(state.op.rotation,"rotation");
+    if (state.more()) {
+        state.consume(state.op.rotation,"rotation");
+    }
     if (!state.failed) {
         state.manager.apply(state.op,state.result);
     }
@@ -425,6 +427,10 @@
                 reply.addDouble(result.location.get(0));
                 reply.addDouble(result.location.get(1));
                 reply.addDouble(result.location.get(2));
+            } else if (result.rotation.isValid()) {
+                reply.addDouble(result.rotation.get(0));
+                reply.addDouble(result.rotation.get(1));
+                reply.addDouble(result.rotation.get(2));
             } else if (result.color.isValid()) {
                 reply.addDouble(result.color.get(0));
                 reply.addDouble(result.color.get(1));
Index: src/simulators/iCubSimulation/wrapper/WorldOp.h
===================================================================
--- src/simulators/iCubSimulation/wrapper/WorldOp.h	(revision 19662)
+++ src/simulators/iCubSimulation/wrapper/WorldOp.h	(working copy)
@@ -138,6 +138,7 @@
     bool success;
     std::string msg;
     WorldOpTriplet location;
+    WorldOpTriplet rotation;
     WorldOpTriplet color;
     WorldOpName path;
     WorldOpIndex count;
Index: src/simulators/iCubSimulation/odesdl/OdeWorldManager.cpp
===================================================================
--- src/simulators/iCubSimulation/odesdl/OdeWorldManager.cpp	(revision 19662)
+++ src/simulators/iCubSimulation/odesdl/OdeWorldManager.cpp	(working copy)
@@ -306,33 +306,53 @@
 
 void OdeLink::doRotate() {
     OdeInit& odeinit = OdeInit::get();
-
+    
     if (!checkObject()) return;
+    if (object==NULL) {
+        result.setFail("no geometry found");
+        return;
+    }
+
+    // We want to get the object rotation
     if (!op.rotation.isValid()) {
-        result.setFail("no rotation set");
-        return;
+        if (bid!=NULL) {
+            odeinit.mutex.wait();
+            const dReal *R = dBodyGetRotation(bid);
+            result.rotation = WorldOpTriplet(atan2(R[9], R[10])*180/M_PI, asin(-R[8])*180/M_PI, atan2(R[4], R[0])*180/M_PI);
+            result.setOk();
+            odeinit.mutex.post();
+            return;
+        }
+        if (gid!=NULL) {
+            odeinit.mutex.wait();
+            const dReal *R = dGeomGetRotation(gid);
+            result.rotation = WorldOpTriplet(atan2(R[9], R[10])*180/M_PI, asin(-R[8])*180/M_PI, atan2(R[4], R[0])*180/M_PI);
+            result.setOk();
+            odeinit.mutex.post();
+            return;
+        }
+        result.setFail("no object found");
+                
+    } else { // We want to set the object rotation
+        
+        dMatrix3 Rtx,Rty,Rtz, Rtmp1,Rtmp2;
+        
+        double rotx = (op.rotation.get(0) * M_PI) / 180;
+        double roty = (op.rotation.get(1) * M_PI) / 180;
+        double rotz = (op.rotation.get(2) * M_PI) / 180;
+        
+        dRFromAxisAndAngle(Rtx,1,0,0,rotx);
+        dRFromAxisAndAngle(Rty,0,1,0,roty);
+        dRFromAxisAndAngle(Rtz,0,0,1,rotz);
+        
+        dMultiply0 (Rtmp1,Rty,Rtz,3,3,3);
+        dMultiply0 (Rtmp2,Rtx,Rtmp1,3,3,3);
+        odeinit.mutex.wait();
+        dGeomSetRotation(object->getGeometry(),Rtmp2);
+        odeinit.mutex.post();
+        result.setOk();
+        
     }
-    if (object==NULL) {
-        result.setFail("no geometry found");
-        return;
-    }
-
-    dMatrix3 Rtx,Rty,Rtz, Rtmp1,Rtmp2;
-    
-    double rotx = (op.rotation.get(0) * M_PI) / 180;
-    double roty = (op.rotation.get(1) * M_PI) / 180;
-    double rotz = (op.rotation.get(2) * M_PI) / 180;
-    
-    dRFromAxisAndAngle(Rtx,1,0,0,rotx);
-    dRFromAxisAndAngle(Rty,0,1,0,roty);
-    dRFromAxisAndAngle(Rtz,0,0,1,rotz);
-    
-    dMultiply0 (Rtmp1,Rty,Rtz,3,3,3);
-    dMultiply0 (Rtmp2,Rtx,Rtmp1,3,3,3);
-    odeinit.mutex.wait();
-    dGeomSetRotation(object->getGeometry(),Rtmp2);
-    odeinit.mutex.post();
-    result.setOk();
 }
 
 void OdeLink::doColor() {
Reply | Threaded
Open this post in threaded view
|

Re: patch: get object orientation in iCub simulator

paulfitz
Administrator

Hi Fabien,

Thanks for posting this patch.  It is now committed, sorry for the delay.

Cheers,
Paul

On 08/28/2012 06:38 PM, Fabien Benureau wrote:

> Hi, I wrote a bit of code to get the object orientation in the iCub simulator
> (previously, you could only set it).
> I am posting it here for review. It's pretty straightforward, and involve
> copy/past/adapt of code for getting position, with the extra hurdle that ode
> gives you a rotation matrix instead of a vector of angles.
>
> As-is, it introduce the following syntax to get object orientation :
>
>
> Example usage :
>
>
> Patch :
>
>
>
>
> --
> View this message in context: http://robotcub-hackers.2198711.n2.nabble.com/patch-get-object-orientation-in-iCub-simulator-tp7577599.html
> Sent from the RobotCub Hackers mailing list archive at Nabble.com.
>
> ------------------------------------------------------------------------------
> Live Security Virtual Conference
> Exclusive live event will cover all the ways today's security and
> threat landscape has changed and how IT managers can respond. Discussions
> will include endpoint security, mobile security and the latest in malware
> threats. http://www.accelacomm.com/jaw/sfrnl04242012/114/50122263/
> _______________________________________________
> Robotcub-hackers mailing list
> [hidden email]
> https://lists.sourceforge.net/lists/listinfo/robotcub-hackers


------------------------------------------------------------------------------
Live Security Virtual Conference
Exclusive live event will cover all the ways today's security and
threat landscape has changed and how IT managers can respond. Discussions
will include endpoint security, mobile security and the latest in malware
threats. http://www.accelacomm.com/jaw/sfrnl04242012/114/50122263/
_______________________________________________
Robotcub-hackers mailing list
[hidden email]
https://lists.sourceforge.net/lists/listinfo/robotcub-hackers