[Fawkes Git] branch/thofmann/robocup-jaco-move-product: created (0.5.0-3097-g46fc7a8)

Till Hofmann hofmann at kbsg.rwth-aachen.de
Thu Jun 23 17:53:52 CEST 2016


Changes have been pushed for the project "Fawkes Robotics Software Framework".

Gitweb: http://git.fawkesrobotics.org/fawkes.git
Trac:   http://trac.fawkesrobotics.org

The branch, thofmann/robocup-jaco-move-product has been created
        at  46fc7a808dc998e0504fb824e850f98dead40402 (commit)

http://git.fawkesrobotics.org/fawkes.git/thofmann/robocup-jaco-move-product

- *Log* ---------------------------------------------------------------
commit c9c362ebdf23cabeeef8d9ad213438a7b72b2b13
Author:     Till Hofmann <hofmann at kbsg.rwth-aachen.de>
AuthorDate: Wed Jun 8 18:30:19 2016 +0200
Commit:     Till Hofmann <hofmann at kbsg.rwth-aachen.de>
CommitDate: Wed Jun 8 18:30:19 2016 +0200

    add skill

http://git.fawkesrobotics.org/fawkes.git/commit/c9c362e
http://trac.fawkesrobotics.org/changeset/c9c362e

- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
commit a78ed0a09dfb421340b1b81c96857a45f18f023e
Author:     Till Hofmann <hofmann at kbsg.rwth-aachen.de>
AuthorDate: Fri Jun 10 14:40:58 2016 +0200
Commit:     Till Hofmann <hofmann at kbsg.rwth-aachen.de>
CommitDate: Fri Jun 10 14:41:39 2016 +0200

    make position of the product on the belt parameterized

http://git.fawkesrobotics.org/fawkes.git/commit/a78ed0a
http://trac.fawkesrobotics.org/changeset/a78ed0a

- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
commit a20a6c6408f255710a9d58e1f82d7b2213d30b67
Author:     Till Hofmann <hofmann at kbsg.rwth-aachen.de>
AuthorDate: Fri Jun 10 14:44:01 2016 +0200
Commit:     Till Hofmann <hofmann at kbsg.rwth-aachen.de>
CommitDate: Fri Jun 10 14:44:01 2016 +0200

    switch to separate model for the product

http://git.fawkesrobotics.org/fawkes.git/commit/a20a6c6
http://trac.fawkesrobotics.org/changeset/a20a6c6

- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
commit e8326e8885d0124501f7d8b12767de911e9aa347
Author:     Till Hofmann <hofmann at kbsg.rwth-aachen.de>
AuthorDate: Fri Jun 10 14:44:38 2016 +0200
Commit:     Till Hofmann <hofmann at kbsg.rwth-aachen.de>
CommitDate: Fri Jun 10 14:44:38 2016 +0200

    adapt to newer version of libkindrv

http://git.fawkesrobotics.org/fawkes.git/commit/e8326e8
http://trac.fawkesrobotics.org/changeset/e8326e8

- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
commit 50f492f377cffcc1725baa4fed033c238519f3ed
Author:     Till Hofmann <hofmann at kbsg.rwth-aachen.de>
AuthorDate: Fri Jun 10 14:45:29 2016 +0200
Commit:     Till Hofmann <hofmann at kbsg.rwth-aachen.de>
CommitDate: Fri Jun 10 14:45:29 2016 +0200

    add parantheses to fix GCC warning

http://git.fawkesrobotics.org/fawkes.git/commit/50f492f
http://trac.fawkesrobotics.org/changeset/50f492f

- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
commit b8130bc38d23d162bb75822a05efd488c12b27e9
Author:     Till Hofmann <hofmann at kbsg.rwth-aachen.de>
AuthorDate: Fri Jun 10 14:56:23 2016 +0200
Commit:     Till Hofmann <hofmann at kbsg.rwth-aachen.de>
CommitDate: Fri Jun 10 14:56:23 2016 +0200

    add RCLL openrave models

http://git.fawkesrobotics.org/fawkes.git/commit/b8130bc
http://trac.fawkesrobotics.org/changeset/b8130bc

- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
commit 5d7ff45fb38df4658d4b6834c9fd041ec974b305
Author:     Till Hofmann <hofmann at kbsg.rwth-aachen.de>
AuthorDate: Fri Jun 10 16:35:36 2016 +0200
Commit:     Till Hofmann <hofmann at kbsg.rwth-aachen.de>
CommitDate: Fri Jun 10 16:35:36 2016 +0200

    If hover over the belt fails, try to hover closer to the belt
    
    Instead of trying to hover even higher, try to hover closer to the belt.
    Usually hovering fails because the arm cannot be moved so high; going
    even higher does not solve anything.

http://git.fawkesrobotics.org/fawkes.git/commit/5d7ff45
http://trac.fawkesrobotics.org/changeset/5d7ff45

- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
commit 19d07cd43c7dbe4fe44a0be78f5acce0162ce02b
Author:     Till Hofmann <hofmann at kbsg.rwth-aachen.de>
AuthorDate: Fri Jun 10 16:51:35 2016 +0200
Commit:     Till Hofmann <hofmann at kbsg.rwth-aachen.de>
CommitDate: Fri Jun 10 16:51:35 2016 +0200

    rcll robot model: add min/max heights

http://git.fawkesrobotics.org/fawkes.git/commit/19d07cd
http://trac.fawkesrobotics.org/changeset/19d07cd

- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
commit e5d250a5e9f6cdac94efad68ef211c51bb7e52ac
Author:     Till Hofmann <hofmann at kbsg.rwth-aachen.de>
AuthorDate: Fri Jun 10 16:56:46 2016 +0200
Commit:     Till Hofmann <hofmann at kbsg.rwth-aachen.de>
CommitDate: Fri Jun 10 16:56:46 2016 +0200

    parameterize drop position

http://git.fawkesrobotics.org/fawkes.git/commit/e5d250a
http://trac.fawkesrobotics.org/changeset/e5d250a

- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
commit 89b18c0b1f0f16cae224895ce7c6453b36647497
Author:     Till Hofmann <hofmann at kbsg.rwth-aachen.de>
AuthorDate: Wed Jun 15 13:05:25 2016 +0200
Commit:     Till Hofmann <hofmann at kbsg.rwth-aachen.de>
CommitDate: Wed Jun 15 13:05:25 2016 +0200

    split gripper close and obj attachment in two states, check grab
    
    Also add a stage on the retract branch to make sure the arm doesn't hit
    the arm while retracting.

http://git.fawkesrobotics.org/fawkes.git/commit/89b18c0
http://trac.fawkesrobotics.org/changeset/89b18c0

- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
commit 16eee25ecfe9fc8d7846ee0f4814be965735cef1
Author:     Till Hofmann <hofmann at kbsg.rwth-aachen.de>
AuthorDate: Wed Jun 15 13:07:24 2016 +0200
Commit:     Till Hofmann <hofmann at kbsg.rwth-aachen.de>
CommitDate: Wed Jun 15 13:07:24 2016 +0200

    parameterize more jaco arm positions
    
    Add constants instead of hard-coding the values.

http://git.fawkesrobotics.org/fawkes.git/commit/16eee25
http://trac.fawkesrobotics.org/changeset/16eee25

- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
commit 293102b8eaadef5c0b0976f8cad6ee364cdecfbd
Author:     Till Hofmann <hofmann at kbsg.rwth-aachen.de>
AuthorDate: Wed Jun 15 13:08:05 2016 +0200
Commit:     Till Hofmann <hofmann at kbsg.rwth-aachen.de>
CommitDate: Wed Jun 15 13:08:05 2016 +0200

    only close two fingers during the grab

http://git.fawkesrobotics.org/fawkes.git/commit/293102b
http://trac.fawkesrobotics.org/changeset/293102b

- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
commit 095508f730b60c7c23270c67db2aeb1b76d1961d
Author:     Till Hofmann <hofmann at kbsg.rwth-aachen.de>
AuthorDate: Wed Jun 15 18:31:56 2016 +0200
Commit:     Till Hofmann <hofmann at kbsg.rwth-aachen.de>
CommitDate: Wed Jun 15 18:44:30 2016 +0200

    adapt to new arm position mounted on the left of the MPS

http://git.fawkesrobotics.org/fawkes.git/commit/095508f
http://trac.fawkesrobotics.org/changeset/095508f

- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
commit ce751a17b138ca370d61cf2936b9c466d02476a2
Author:     Till Hofmann <hofmann at kbsg.rwth-aachen.de>
AuthorDate: Wed Jun 15 18:46:52 2016 +0200
Commit:     Till Hofmann <hofmann at kbsg.rwth-aachen.de>
CommitDate: Wed Jun 15 18:46:52 2016 +0200

    add separators to limit the movement of the arm

http://git.fawkesrobotics.org/fawkes.git/commit/ce751a1
http://trac.fawkesrobotics.org/changeset/ce751a1

- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
commit 6b1c0e1797e3ffdfed39e04e692d998e91741e5c
Author:     Till Hofmann <hofmann at kbsg.rwth-aachen.de>
AuthorDate: Wed Jun 15 19:02:14 2016 +0200
Commit:     Till Hofmann <hofmann at kbsg.rwth-aachen.de>
CommitDate: Wed Jun 15 19:02:14 2016 +0200

    add a box at the drop location

http://git.fawkesrobotics.org/fawkes.git/commit/6b1c0e1
http://trac.fawkesrobotics.org/changeset/6b1c0e1

- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
commit 46fc7a808dc998e0504fb824e850f98dead40402
Author:     Till Hofmann <hofmann at kbsg.rwth-aachen.de>
AuthorDate: Thu Jun 23 17:52:20 2016 +0200
Commit:     Till Hofmann <hofmann at kbsg.rwth-aachen.de>
CommitDate: Thu Jun 23 17:52:20 2016 +0200

    add model for a simple box where the product is placed
    
    This will be replaced by the actual base of @work.

http://git.fawkesrobotics.org/fawkes.git/commit/46fc7a8
http://trac.fawkesrobotics.org/changeset/46fc7a8

- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -


- *Summary* -----------------------------------------------------------
 .../{plate.kinbody.xml => dropbox.kinbody.xml}     |    8 +-
 res/openrave/mps_cap.stl                           |  Bin 0 -> 493384 bytes
 res/openrave/rcll.machine.xml                      |   11 ++
 .../{cylinder_red.kinbody.xml => rcll.prod.xml}    |    4 +-
 res/openrave/rcll.robot.xml                        |   51 ++++++++
 .../skills/generic/advanced/rcll_move_product.lua  |  127 +++++++++++++++-----
 src/plugins/jaco/arm_kindrv.cpp                    |   10 +-
 src/plugins/jaco/openrave_thread.cpp               |    2 +-
 8 files changed, 172 insertions(+), 41 deletions(-)
 copy res/openrave/{plate.kinbody.xml => dropbox.kinbody.xml} (53%)
 create mode 100644 res/openrave/mps_cap.stl
 create mode 100644 res/openrave/rcll.machine.xml
 copy res/openrave/{cylinder_red.kinbody.xml => rcll.prod.xml} (83%)
 create mode 100644 res/openrave/rcll.robot.xml


- *Diffs* -------------------------------------------------------------

- *commit* c9c362ebdf23cabeeef8d9ad213438a7b72b2b13 - - - - - - - - - -
Author:  Till Hofmann <hofmann at kbsg.rwth-aachen.de>
Date:    Wed Jun 8 18:30:19 2016 +0200
Subject: add skill

 .../skills/generic/advanced/rcll_move_product.lua  |  229 ++++++++++++++++++++
 1 files changed, 229 insertions(+), 0 deletions(-)


- *commit* a78ed0a09dfb421340b1b81c96857a45f18f023e - - - - - - - - - -
Author:  Till Hofmann <hofmann at kbsg.rwth-aachen.de>
Date:    Fri Jun 10 14:40:58 2016 +0200
Subject: make position of the product on the belt parameterized

 .../skills/generic/advanced/rcll_move_product.lua  |   24 ++++++++++++++-----
 1 files changed, 17 insertions(+), 7 deletions(-)

_Diff for modified files_:
diff --git a/src/lua/skills/generic/advanced/rcll_move_product.lua b/src/lua/skills/generic/advanced/rcll_move_product.lua
index 443c3c3..e9b6b18 100644
--- a/src/lua/skills/generic/advanced/rcll_move_product.lua
+++ b/src/lua/skills/generic/advanced/rcll_move_product.lua
@@ -46,6 +46,9 @@ local PARK_Z = 0.9
 local PARK_E1 = 1.57
 local PARK_E2 = 3.14
 local PARK_E3 = 3.14
+local PROD_POS_X = 0.25
+local PROD_POS_Y = 0.22
+local PROD_POS_Z = 0.91
 
 -- States
 fsm:define_states{ export_to=_M,
@@ -136,8 +139,9 @@ end
 
 function MOVE_OBJS:init()
   self.args["or_object"] = {
-    move={{name="mps", x=0.25, y=0.3, z=0.0},
-          {name="prod", x=0.25,y=0.4,z=0.84}}}
+    move={{name="mps", x=0.55, y=0.6, z=0},
+          {name="prod", x=PROD_POS_X,y=PROD_POS_Y,z=PROD_POS_Z}
+          }}
 end
 
 function INIT_PARK:init()
@@ -150,19 +154,24 @@ function OPEN_GRIPPER:init()
 end
 
 function HOVER_BELT:init()
-  self.args["jaco"] = {x=0.25,y=0.4,z=0.90,e1=1.57,e2=3.14,e3=3.14}
+  self.args["jaco"] = {x=PROD_POS_X,y=PROD_POS_Y,z=PROD_POS_Z+0.1,
+                        e1=1.57,e2=3.14,e3=3.14}
 end
 
 function RELAXED_HOVER_BELT:init()
-  self.args["jaco"] = {x=0.25,y=0.4,z=0.95,e1=1.57,e2=3.14,e3=3.14}
+  self.args["jaco"] = {x=PROD_POS_X,y=PROD_POS_Y,z=PROD_POS_Z+0.2,
+                        e1=1.57,e2=3.14,e3=3.14}
+
 end
 
 function BELT_GRAB_POS:init()
-  self.args["jaco"] = {x=0.25,y=0.4,z=0.83,e1=1.57,e2=3.14,e3=3.14}
+  self.args["jaco"] = {x=PROD_POS_X,y=PROD_POS_Y,z=PROD_POS_Z+0.02,
+                        e1=1.57,e2=3.14,e3=3.14}
 end
 
 function RELAXED_BELT_GRAB_POS:init()
-  self.args["jaco"] = {x=0.25,y=0.4,z=0.87,e1=1.57,e2=3.14,e3=3.14}
+  self.args["jaco"] = {x=PROD_POS_X,y=PROD_POS_Y,z=PROD_POS_Z+0.05,
+                        e1=1.57,e2=3.14,e3=3.14}
 end
 
 function GRAB_FROM_BELT:init()
@@ -171,7 +180,8 @@ function GRAB_FROM_BELT:init()
 end
 
 function MOVE_UP_FROM_BELT:init()
-  self.args["jaco"] = {x=0.25, y=0.4, z=0.95,e1=1.57,e2=3.14,e3=3.14}
+  self.args["jaco"] = {x=PROD_POS_X,y=PROD_POS_Y,z=PROD_POS_Z+0.1,
+                        e1=1.57,e2=3.14,e3=3.14}
 end
 
 function MOVE_TO_MIDDLE:init()

- *commit* a20a6c6408f255710a9d58e1f82d7b2213d30b67 - - - - - - - - - -
Author:  Till Hofmann <hofmann at kbsg.rwth-aachen.de>
Date:    Fri Jun 10 14:44:01 2016 +0200
Subject: switch to separate model for the product

 .../skills/generic/advanced/rcll_move_product.lua  |    2 +-
 1 files changed, 1 insertions(+), 1 deletions(-)

_Diff for modified files_:
diff --git a/src/lua/skills/generic/advanced/rcll_move_product.lua b/src/lua/skills/generic/advanced/rcll_move_product.lua
index e9b6b18..ecc6947 100644
--- a/src/lua/skills/generic/advanced/rcll_move_product.lua
+++ b/src/lua/skills/generic/advanced/rcll_move_product.lua
@@ -134,7 +134,7 @@ end
 function ADD_OBJS:init()
   self.args["or_object"] = {
     add={ {name="mps", path="rcll.machine.xml"},
-          {name="prod", path="cylinder_red.kinbody.xml"}}}
+          {name="prod", path="rcll.prod.xml"}}}
 end
 
 function MOVE_OBJS:init()

- *commit* e8326e8885d0124501f7d8b12767de911e9aa347 - - - - - - - - - -
Author:  Till Hofmann <hofmann at kbsg.rwth-aachen.de>
Date:    Fri Jun 10 14:44:38 2016 +0200
Subject: adapt to newer version of libkindrv

 src/plugins/jaco/arm_kindrv.cpp |   10 +++++-----
 1 files changed, 5 insertions(+), 5 deletions(-)

_Diff for modified files_:
diff --git a/src/plugins/jaco/arm_kindrv.cpp b/src/plugins/jaco/arm_kindrv.cpp
index 609ecab..e40ecd9 100644
--- a/src/plugins/jaco/arm_kindrv.cpp
+++ b/src/plugins/jaco/arm_kindrv.cpp
@@ -107,7 +107,7 @@ JacoArmKindrv::final()
 
     case TARGET_READY:
       {
-        jaco_retract_mode_t mode = __arm->get_status();
+        jaco_retract_mode_t mode = __arm->get_status().retract;
         __final = (mode == MODE_READY_STANDBY);
 
         if( __final ) {
@@ -122,7 +122,7 @@ JacoArmKindrv::final()
 
     case TARGET_RETRACT:
       {
-        jaco_retract_mode_t mode = __arm->get_status();
+        jaco_retract_mode_t mode = __arm->get_status().retract;
         __final = (mode == MODE_RETRACT_STANDBY);
       }
       if( __final )
@@ -150,7 +150,7 @@ bool
 JacoArmKindrv::initialized()
 {
   if( !__initialized ) {
-    jaco_retract_mode_t mode = __arm->get_status();
+    jaco_retract_mode_t mode = __arm->get_status().retract;
     __initialized = (mode != MODE_NOINIT);
   }
 
@@ -284,7 +284,7 @@ JacoArmKindrv::goto_ready()
   __final = false;
 
   __arm->start_api_ctrl();
-  jaco_retract_mode_t mode = __arm->get_status();
+  jaco_retract_mode_t mode = __arm->get_status().retract;
   switch( mode ) {
     case MODE_RETRACT_TO_READY:
       //2 buttons needed
@@ -321,7 +321,7 @@ JacoArmKindrv::goto_retract()
   __final = false;
 
   __arm->start_api_ctrl();
-  jaco_retract_mode_t mode = __arm->get_status();
+  jaco_retract_mode_t mode = __arm->get_status().retract;
   switch( mode ) {
     case MODE_READY_TO_RETRACT:
       // 2 buttons needed

- *commit* 50f492f377cffcc1725baa4fed033c238519f3ed - - - - - - - - - -
Author:  Till Hofmann <hofmann at kbsg.rwth-aachen.de>
Date:    Fri Jun 10 14:45:29 2016 +0200
Subject: add parantheses to fix GCC warning

 src/plugins/jaco/openrave_thread.cpp |    2 +-
 1 files changed, 1 insertions(+), 1 deletions(-)

_Diff for modified files_:
diff --git a/src/plugins/jaco/openrave_thread.cpp b/src/plugins/jaco/openrave_thread.cpp
index 1cd2c5f..cf440d6 100644
--- a/src/plugins/jaco/openrave_thread.cpp
+++ b/src/plugins/jaco/openrave_thread.cpp
@@ -258,7 +258,7 @@ JacoOpenraveThread::loop()
       } else if( (*it)->trajec_state==TRAJEC_SKIP && (*it)->type == TARGET_ANGULAR ) {
         from = *it;
         break;
-      } else if( !(*it)->type==TARGET_GRIPPER ) {
+      } else if( !((*it)->type==TARGET_GRIPPER) ) {
         // A previous target has unknown final configuration. Cannot plan for our target yet. Abort.
         //  TARGET_GRIPPER would be the only one we could skip without problems.
         __arm->target_mutex->unlock();

- *commit* b8130bc38d23d162bb75822a05efd488c12b27e9 - - - - - - - - - -
Author:  Till Hofmann <hofmann at kbsg.rwth-aachen.de>
Date:    Fri Jun 10 14:56:23 2016 +0200
Subject: add RCLL openrave models

 res/openrave/mps_cap.stl                           |  Bin 0 -> 493384 bytes
 res/openrave/rcll.machine.xml                      |   10 ++++
 .../{cylinder_red.kinbody.xml => rcll.prod.xml}    |    4 +-
 res/openrave/rcll.robot.xml                        |   49 ++++++++++++++++++++
 4 files changed, 61 insertions(+), 2 deletions(-)


- *commit* 5d7ff45fb38df4658d4b6834c9fd041ec974b305 - - - - - - - - - -
Author:  Till Hofmann <hofmann at kbsg.rwth-aachen.de>
Date:    Fri Jun 10 16:35:36 2016 +0200
Subject: If hover over the belt fails, try to hover closer to the belt

 .../skills/generic/advanced/rcll_move_product.lua  |    8 ++++----
 1 files changed, 4 insertions(+), 4 deletions(-)

_Diff for modified files_:
diff --git a/src/lua/skills/generic/advanced/rcll_move_product.lua b/src/lua/skills/generic/advanced/rcll_move_product.lua
index ecc6947..1aee75a 100644
--- a/src/lua/skills/generic/advanced/rcll_move_product.lua
+++ b/src/lua/skills/generic/advanced/rcll_move_product.lua
@@ -78,8 +78,8 @@ fsm:define_states{ export_to=_M,
   -- further above the belt.
   -- If the grab fails, open the gripper and retract to parking.
   {"HOVER_BELT", SkillJumpState, final_to="BELT_GRAB_POS",
-    fail_to="RELAXED_HOVER_BELT", skills={{jaco}}},
-  {"RELAXED_HOVER_BELT", SkillJumpState, final_to="BELT_GRAB_POS",
+    fail_to="CLOSE_HOVER_BELT", skills={{jaco}}},
+  {"CLOSE_HOVER_BELT", SkillJumpState, final_to="BELT_GRAB_POS",
     fail_to="FAILED", skills={{jaco}}},
   {"BELT_GRAB_POS", SkillJumpState, final_to="GRAB_FROM_BELT",
     fail_to="RELAXED_BELT_GRAB_POS", skills={{jaco}}},
@@ -158,8 +158,8 @@ function HOVER_BELT:init()
                         e1=1.57,e2=3.14,e3=3.14}
 end
 
-function RELAXED_HOVER_BELT:init()
-  self.args["jaco"] = {x=PROD_POS_X,y=PROD_POS_Y,z=PROD_POS_Z+0.2,
+function CLOSE_HOVER_BELT:init()
+  self.args["jaco"] = {x=PROD_POS_X,y=PROD_POS_Y,z=PROD_POS_Z+0.05,
                         e1=1.57,e2=3.14,e3=3.14}
 
 end

- *commit* 19d07cd43c7dbe4fe44a0be78f5acce0162ce02b - - - - - - - - - -
Author:  Till Hofmann <hofmann at kbsg.rwth-aachen.de>
Date:    Fri Jun 10 16:51:35 2016 +0200
Subject: rcll robot model: add min/max heights

 res/openrave/rcll.robot.xml |    2 ++
 1 files changed, 2 insertions(+), 0 deletions(-)

_Diff for modified files_:
diff --git a/res/openrave/rcll.robot.xml b/res/openrave/rcll.robot.xml
index c8112e3..2bc8b50 100644
--- a/res/openrave/rcll.robot.xml
+++ b/res/openrave/rcll.robot.xml
@@ -2,6 +2,8 @@
 <Robot name="jaco_robot">
 
   <Robot prefix="arm_" file="jaco.kinbody.xml" name="jaco_arm">
+    <!-- min: 0.53
+         max: 0.63 -->
     <Translation> 0 0 0.56</Translation>
     <RotationAxis>0 0 1 270</RotationAxis>
   </Robot>

- *commit* e5d250a5e9f6cdac94efad68ef211c51bb7e52ac - - - - - - - - - -
Author:  Till Hofmann <hofmann at kbsg.rwth-aachen.de>
Date:    Fri Jun 10 16:56:46 2016 +0200
Subject: parameterize drop position

 .../skills/generic/advanced/rcll_move_product.lua  |   12 +++++++++---
 1 files changed, 9 insertions(+), 3 deletions(-)

_Diff for modified files_:
diff --git a/src/lua/skills/generic/advanced/rcll_move_product.lua b/src/lua/skills/generic/advanced/rcll_move_product.lua
index 1aee75a..5a022e3 100644
--- a/src/lua/skills/generic/advanced/rcll_move_product.lua
+++ b/src/lua/skills/generic/advanced/rcll_move_product.lua
@@ -49,6 +49,9 @@ local PARK_E3 = 3.14
 local PROD_POS_X = 0.25
 local PROD_POS_Y = 0.22
 local PROD_POS_Z = 0.91
+local DROP_POS_X = -0.1
+local DROP_POS_Y = 0.4
+local DROP_POS_Z = 0.1
 
 -- States
 fsm:define_states{ export_to=_M,
@@ -189,11 +192,13 @@ function MOVE_TO_MIDDLE:init()
 end
 
 function HOVER_GROUND:init()
-  self.args["jaco"] = {x=-0.1, y=0.4, z=0.2,e1=1.57,e2=3.14,e3=3.14}
+  self.args["jaco"] = {x=DROP_POS_X, y=DROP_POS_Y, z=DROP_POS_Z+0.12,
+                       e1=1.57,e2=3.14,e3=3.14}
 end
 
 function DROP_POS:init()
-  self.args["jaco"] = {x=-0.1, y=0.4, z=0.1,e1=1.57,e2=3.14,e3=3.14}
+  self.args["jaco"] = {x=DROP_POS_X, y=DROP_POS_Y, z=DROP_POS_Z+0.02,
+                       e1=1.57,e2=3.14,e3=3.14}
 end
 
 function DROP:init()
@@ -202,7 +207,8 @@ function DROP:init()
 end
 
 function POST_DROP:init()
-  self.args["jaco"] = {x=-0.1, y=0.4, z=0.2,e1=1.57,e2=3.14,e3=3.14}
+  self.args["jaco"] = {x=DROP_POS_X, y=DROP_POS_Y, z=DROP_POS_Z+0.12,
+                       e1=1.57,e2=3.14,e3=3.14}
 end
 
 function PARK:init()

- *commit* 89b18c0b1f0f16cae224895ce7c6453b36647497 - - - - - - - - - -
Author:  Till Hofmann <hofmann at kbsg.rwth-aachen.de>
Date:    Wed Jun 15 13:05:25 2016 +0200
Subject: split gripper close and obj attachment in two states, check grab

 .../skills/generic/advanced/rcll_move_product.lua  |   33 +++++++++++++++++--
 1 files changed, 29 insertions(+), 4 deletions(-)

_Diff for modified files_:
diff --git a/src/lua/skills/generic/advanced/rcll_move_product.lua b/src/lua/skills/generic/advanced/rcll_move_product.lua
index 5a022e3..8c1f5c9 100644
--- a/src/lua/skills/generic/advanced/rcll_move_product.lua
+++ b/src/lua/skills/generic/advanced/rcll_move_product.lua
@@ -88,8 +88,11 @@ fsm:define_states{ export_to=_M,
     fail_to="RELAXED_BELT_GRAB_POS", skills={{jaco}}},
   {"RELAXED_BELT_GRAB_POS", SkillJumpState, final_to="GRAB_FROM_BELT",
     fail_to="FAILED", skills={{jaco}}},
-  {"GRAB_FROM_BELT", SkillJumpState, final_to="MOVE_UP_FROM_BELT",
-    fail_to="RETRACT_OPEN_GRIPPER", skills={{jaco},{or_object}}},
+  {"GRAB_FROM_BELT", SkillJumpState, final_to="CHECK_GRAB",
+    fail_to="RETRACT_OPEN_GRIPPER", skills={{jaco}}},
+  {"CHECK_GRAB", JumpState},
+  {"ATTACH_PROD", SkillJumpState, final_to="MOVE_UP_FROM_BELT",
+    fail_to="RETRACT_OPEN_GRIPPER", skills={{or_object}}},
   {"MOVE_UP_FROM_BELT", SkillJumpState, final_to="MOVE_TO_MIDDLE",
     fail_to="FAILED", skills={{jaco}}},
   -- intermediate position which is above the drop position
@@ -116,8 +119,10 @@ fsm:define_states{ export_to=_M,
     skills={{jaco}}},
   {"RETRACT_TO_PARK", SkillJumpState, final_to="FAILED",
     fail_to="RETRACT_TO_HOME", skills={{jaco}}},
-  {"RETRACT_OPEN_GRIPPER", SkillJumpState, final_to="RETRACT_TO_PARK",
-    fail_to="RETRACT_TO_PARK", skills={{jaco}}},
+  {"RETRACT_OPEN_GRIPPER", SkillJumpState, final_to="RETRACT_MOVE_UP_FROM_MPS",
+    fail_to="RETRACT_MOVE_UP_FROM_MPS", skills={{jaco}}},
+  {"RETRACT_MOVE_UP_FROM_MPS", SkillJumpState, final_to="RETRACT_TO_PARK",
+    fail_to="FAILED", skills={{jaco}}},
   {"RETRACT_MOVE_UP", SkillJumpState, final_to="RETRACT_TO_PARK",
     fail_to="FAILED", skills={{jaco}}},
   {"RETRY_PARK_INTERMEDIATE", SkillJumpState, final_to="RETRY_PARK",
@@ -128,6 +133,8 @@ fsm:define_states{ export_to=_M,
 
 -- Transitions
 fsm:add_transitions{
+  {"CHECK_GRAB", "ATTACH_PROD", cond="self.success"},
+  {"CHECK_GRAB", "RETRACT_OPEN_GRIPPER", cond=true}
 }
 
 function START:init()
@@ -179,9 +186,22 @@ end
 
 function GRAB_FROM_BELT:init()
   self.args["jaco"] = {gripper="close"}
+end
+
+function ATTACH_PROD:init()
   self.args["or_object"] = {attach={"prod"}}
 end
 
+
+function CHECK_GRAB:init()
+  local f1, f2 = jacoarm:finger1(), jacoarm:finger2()
+  if f1 <= 30 or f1 >= 50 or f2 <= 30 or f2 >= 50 then
+    self.success = false
+  else
+    self.success = true
+  end
+end
+
 function MOVE_UP_FROM_BELT:init()
   self.args["jaco"] = {x=PROD_POS_X,y=PROD_POS_Y,z=PROD_POS_Z+0.1,
                         e1=1.57,e2=3.14,e3=3.14}
@@ -230,6 +250,11 @@ function RETRACT_OPEN_GRIPPER:init()
   self.args["jaco"] = {gripper="open"}
 end
 
+function RETRACT_MOVE_UP_FROM_MPS:init()
+  self.args["jaco"] = {x=PROD_POS_X, y=PROD_POS_Y, z=PROD_POS_Z+0.1,
+                        e1=1.57,e2=3.14,e3=PICK_UP_E3}
+end
+
 function RETRACT_MOVE_UP:init()
   self.args["jaco"] = {x=-0.1, y=0.4, z=0.2,e1=1.57,e2=3.14,e3=3.14}
 end

- *commit* 16eee25ecfe9fc8d7846ee0f4814be965735cef1 - - - - - - - - - -
Author:  Till Hofmann <hofmann at kbsg.rwth-aachen.de>
Date:    Wed Jun 15 13:07:24 2016 +0200
Subject: parameterize more jaco arm positions

 .../skills/generic/advanced/rcll_move_product.lua  |   42 ++++++++++---------
 1 files changed, 22 insertions(+), 20 deletions(-)

_Diff for modified files_:
diff --git a/src/lua/skills/generic/advanced/rcll_move_product.lua b/src/lua/skills/generic/advanced/rcll_move_product.lua
index 8c1f5c9..4755495 100644
--- a/src/lua/skills/generic/advanced/rcll_move_product.lua
+++ b/src/lua/skills/generic/advanced/rcll_move_product.lua
@@ -3,7 +3,7 @@
 --  rcll_move_product.lua -- Skill to grab a product from the MPS
 --
 --  Created: Tue Jun 07 17:44:42 2016
---  Copyright  2016  Till Hofmann 
+--  Copyright  2016  Till Hofmann
 --
 ----------------------------------------------------------------------------
 
@@ -42,16 +42,18 @@ skillenv.skill_module(_M)
 --local TARGET_REACHED_DISTANCE = 0.8
 local PARK_X = 0.0
 local PARK_Y = 0.4
-local PARK_Z = 0.9
+local PARK_Z = 1.0
 local PARK_E1 = 1.57
 local PARK_E2 = 3.14
 local PARK_E3 = 3.14
 local PROD_POS_X = 0.25
-local PROD_POS_Y = 0.22
+local PROD_POS_Y = 0.20
 local PROD_POS_Z = 0.91
+local PICK_UP_E3 = 3.4
+local PICK_UP_Y_OFFSET = 0.02
 local DROP_POS_X = -0.1
 local DROP_POS_Y = 0.4
-local DROP_POS_Z = 0.1
+local DROP_POS_Z = 0.3
 
 -- States
 fsm:define_states{ export_to=_M,
@@ -164,24 +166,24 @@ function OPEN_GRIPPER:init()
 end
 
 function HOVER_BELT:init()
-  self.args["jaco"] = {x=PROD_POS_X,y=PROD_POS_Y,z=PROD_POS_Z+0.1,
-                        e1=1.57,e2=3.14,e3=3.14}
+  self.args["jaco"] = {x=PROD_POS_X,y=PROD_POS_Y+PICK_UP_Y_OFFSET,z=PROD_POS_Z+0.1,
+                        e1=1.57,e2=3.14,e3=PICK_UP_E3}
 end
 
 function CLOSE_HOVER_BELT:init()
-  self.args["jaco"] = {x=PROD_POS_X,y=PROD_POS_Y,z=PROD_POS_Z+0.05,
-                        e1=1.57,e2=3.14,e3=3.14}
+  self.args["jaco"] = {x=PROD_POS_X,y=PROD_POS_Y+PICK_UP_Y_OFFSET,z=PROD_POS_Z+0.05,
+                        e1=1.57,e2=3.14,e3=PICK_UP_E3}
 
 end
 
 function BELT_GRAB_POS:init()
-  self.args["jaco"] = {x=PROD_POS_X,y=PROD_POS_Y,z=PROD_POS_Z+0.02,
-                        e1=1.57,e2=3.14,e3=3.14}
+  self.args["jaco"] = {x=PROD_POS_X,y=PROD_POS_Y+PICK_UP_Y_OFFSET,z=PROD_POS_Z+0.02,
+                        e1=1.57,e2=3.14,e3=PICK_UP_E3}
 end
 
 function RELAXED_BELT_GRAB_POS:init()
-  self.args["jaco"] = {x=PROD_POS_X,y=PROD_POS_Y,z=PROD_POS_Z+0.05,
-                        e1=1.57,e2=3.14,e3=3.14}
+  self.args["jaco"] = {x=PROD_POS_X,y=PROD_POS_Y+PICK_UP_Y_OFFSET,z=PROD_POS_Z+0.05,
+                        e1=1.57,e2=3.14,e3=PICK_UP_E3}
 end
 
 function GRAB_FROM_BELT:init()
@@ -203,22 +205,22 @@ function CHECK_GRAB:init()
 end
 
 function MOVE_UP_FROM_BELT:init()
-  self.args["jaco"] = {x=PROD_POS_X,y=PROD_POS_Y,z=PROD_POS_Z+0.1,
-                        e1=1.57,e2=3.14,e3=3.14}
+  self.args["jaco"] = {x=PROD_POS_X,y=PROD_POS_Y+PICK_UP_Y_OFFSET,z=PROD_POS_Z+0.2,
+                        e1=1.57,e2=3.14,e3=PICK_UP_E3}
 end
 
 function MOVE_TO_MIDDLE:init()
-  self.args["jaco"] = {x=-0.1, y=0.4, z=0.95,e1=1.57,e2=3.14,e3=3.14} 
+  self.args["jaco"] = {x=-0.1, y=0.4, z=0.95,e1=1.57,e2=3.14,e3=PICK_UP_E3}
 end
 
 function HOVER_GROUND:init()
   self.args["jaco"] = {x=DROP_POS_X, y=DROP_POS_Y, z=DROP_POS_Z+0.12,
-                       e1=1.57,e2=3.14,e3=3.14}
+                       e1=1.57,e2=3.14,e3=PICK_UP_E3}
 end
 
 function DROP_POS:init()
   self.args["jaco"] = {x=DROP_POS_X, y=DROP_POS_Y, z=DROP_POS_Z+0.02,
-                       e1=1.57,e2=3.14,e3=3.14}
+                       e1=1.57,e2=3.14,e3=PICK_UP_E3}
 end
 
 function DROP:init()
@@ -228,7 +230,7 @@ end
 
 function POST_DROP:init()
   self.args["jaco"] = {x=DROP_POS_X, y=DROP_POS_Y, z=DROP_POS_Z+0.12,
-                       e1=1.57,e2=3.14,e3=3.14}
+                       e1=1.57,e2=3.14,e3=PICK_UP_E3}
 end
 
 function PARK:init()
@@ -256,11 +258,11 @@ function RETRACT_MOVE_UP_FROM_MPS:init()
 end
 
 function RETRACT_MOVE_UP:init()
-  self.args["jaco"] = {x=-0.1, y=0.4, z=0.2,e1=1.57,e2=3.14,e3=3.14}
+  self.args["jaco"] = {x=-0.1, y=0.4, z=0.2,e1=1.57,e2=3.14,e3=PICK_UP_E3}
 end
 
 function RETRY_PARK_INTERMEDIATE:init()
-  self.args["jaco"] = {x=-0.1, y=0.4, z=0.4,e1=1.57,e2=3.14,e3=3.14}
+  self.args["jaco"] = {x=-0.1, y=0.4, z=0.4,e1=1.57,e2=3.14,e3=PICK_UP_E3}
 end
 
 function RETRY_PARK:init()

- *commit* 293102b8eaadef5c0b0976f8cad6ee364cdecfbd - - - - - - - - - -
Author:  Till Hofmann <hofmann at kbsg.rwth-aachen.de>
Date:    Wed Jun 15 13:08:05 2016 +0200
Subject: only close two fingers during the grab

 .../skills/generic/advanced/rcll_move_product.lua  |    2 +-
 1 files changed, 1 insertions(+), 1 deletions(-)

_Diff for modified files_:
diff --git a/src/lua/skills/generic/advanced/rcll_move_product.lua b/src/lua/skills/generic/advanced/rcll_move_product.lua
index 4755495..1defbe8 100644
--- a/src/lua/skills/generic/advanced/rcll_move_product.lua
+++ b/src/lua/skills/generic/advanced/rcll_move_product.lua
@@ -187,7 +187,7 @@ function RELAXED_BELT_GRAB_POS:init()
 end
 
 function GRAB_FROM_BELT:init()
-  self.args["jaco"] = {gripper="close"}
+  self.args["jaco"] = {gripper={f1=60,f2=60,f3=20}}
 end
 
 function ATTACH_PROD:init()

- *commit* 095508f730b60c7c23270c67db2aeb1b76d1961d - - - - - - - - - -
Author:  Till Hofmann <hofmann at kbsg.rwth-aachen.de>
Date:    Wed Jun 15 18:31:56 2016 +0200
Subject: adapt to new arm position mounted on the left of the MPS

 res/openrave/rcll.machine.xml                      |    1 +
 res/openrave/rcll.robot.xml                        |    8 ++--
 .../skills/generic/advanced/rcll_move_product.lua  |   47 ++++++++++++--------
 3 files changed, 33 insertions(+), 23 deletions(-)

_Diff for modified files_:
diff --git a/res/openrave/rcll.machine.xml b/res/openrave/rcll.machine.xml
index 29f273c..6a5e21a 100644
--- a/res/openrave/rcll.machine.xml
+++ b/res/openrave/rcll.machine.xml
@@ -7,4 +7,5 @@
     
   </Body>
   <translation>0 0 0</translation>
+  <RotationAxis>0 0 1 90</RotationAxis>
 </KinBody> 
diff --git a/res/openrave/rcll.robot.xml b/res/openrave/rcll.robot.xml
index 2bc8b50..6a95fef 100644
--- a/res/openrave/rcll.robot.xml
+++ b/res/openrave/rcll.robot.xml
@@ -4,8 +4,8 @@
   <Robot prefix="arm_" file="jaco.kinbody.xml" name="jaco_arm">
     <!-- min: 0.53
          max: 0.63 -->
-    <Translation> 0 0 0.56</Translation>
-    <RotationAxis>0 0 1 270</RotationAxis>
+    <Translation> 0 0 0.59</Translation>
+    <RotationAxis>0 0 1 90</RotationAxis>
   </Robot>
 
 
@@ -22,9 +22,9 @@
   <KinBody name="body">
     <Body name="main_body" type="dynamic">
       <Geom type="box">
-        <Extents>0.2 0.1 0.28</Extents>
+        <Extents>0.2 0.1 0.20</Extents>
       </Geom>
-      <Translation>0 0 0.28</Translation>
+      <Translation>0 0 0.20</Translation>
     </Body>
     <!--
     <Body name="mount" type="dynamic">
diff --git a/src/lua/skills/generic/advanced/rcll_move_product.lua b/src/lua/skills/generic/advanced/rcll_move_product.lua
index 1defbe8..7c072f2 100644
--- a/src/lua/skills/generic/advanced/rcll_move_product.lua
+++ b/src/lua/skills/generic/advanced/rcll_move_product.lua
@@ -40,20 +40,21 @@ skillenv.skill_module(_M)
 
 -- Constants and variables
 --local TARGET_REACHED_DISTANCE = 0.8
-local PARK_X = 0.0
-local PARK_Y = 0.4
+local PARK_X = 0.2
+local PARK_Y = -0.1
 local PARK_Z = 1.0
 local PARK_E1 = 1.57
 local PARK_E2 = 3.14
-local PARK_E3 = 3.14
-local PROD_POS_X = 0.25
-local PROD_POS_Y = 0.20
-local PROD_POS_Z = 0.91
-local PICK_UP_E3 = 3.4
-local PICK_UP_Y_OFFSET = 0.02
-local DROP_POS_X = -0.1
-local DROP_POS_Y = 0.4
-local DROP_POS_Z = 0.3
+local PARK_E3 = 1.75
+local PROD_POS_X = 0.28
+local PROD_POS_Y = 0.15
+local PROD_POS_Z = 0.915
+local PICK_UP_E3 = 1.75
+local PICK_UP_X_OFFSET = -0.01
+local PICK_UP_Y_OFFSET = 0.00
+local DROP_POS_X = 0.2
+local DROP_POS_Y = -0.2
+local DROP_POS_Z = 0.1
 
 -- States
 fsm:define_states{ export_to=_M,
@@ -96,6 +97,8 @@ fsm:define_states{ export_to=_M,
   {"ATTACH_PROD", SkillJumpState, final_to="MOVE_UP_FROM_BELT",
     fail_to="RETRACT_OPEN_GRIPPER", skills={{or_object}}},
   {"MOVE_UP_FROM_BELT", SkillJumpState, final_to="MOVE_TO_MIDDLE",
+    fail_to="CLOSE_MOVE_UP_FROM_BELT", skills={{jaco}}},
+  {"CLOSE_MOVE_UP_FROM_BELT", SkillJumpState, final_to="MOVE_TO_MIDDLE",
     fail_to="FAILED", skills={{jaco}}},
   -- intermediate position which is above the drop position
   {"MOVE_TO_MIDDLE", SkillJumpState, final_to="HOVER_GROUND",
@@ -146,12 +149,13 @@ end
 function ADD_OBJS:init()
   self.args["or_object"] = {
     add={ {name="mps", path="rcll.machine.xml"},
-          {name="prod", path="rcll.prod.xml"}}}
+          {name="prod", path="rcll.prod.xml"}
+          }}
 end
 
 function MOVE_OBJS:init()
   self.args["or_object"] = {
-    move={{name="mps", x=0.55, y=0.6, z=0},
+    move={{name="mps", x=-0.10, y=0.47, z=0},
           {name="prod", x=PROD_POS_X,y=PROD_POS_Y,z=PROD_POS_Z}
           }}
 end
@@ -166,23 +170,23 @@ function OPEN_GRIPPER:init()
 end
 
 function HOVER_BELT:init()
-  self.args["jaco"] = {x=PROD_POS_X,y=PROD_POS_Y+PICK_UP_Y_OFFSET,z=PROD_POS_Z+0.1,
+  self.args["jaco"] = {x=PROD_POS_X+PICK_UP_X_OFFSET,y=PROD_POS_Y+PICK_UP_Y_OFFSET,z=PROD_POS_Z+0.1,
                         e1=1.57,e2=3.14,e3=PICK_UP_E3}
 end
 
 function CLOSE_HOVER_BELT:init()
-  self.args["jaco"] = {x=PROD_POS_X,y=PROD_POS_Y+PICK_UP_Y_OFFSET,z=PROD_POS_Z+0.05,
+  self.args["jaco"] = {x=PROD_POS_X+PICK_UP_X_OFFSET,y=PROD_POS_Y+PICK_UP_Y_OFFSET,z=PROD_POS_Z+0.05,
                         e1=1.57,e2=3.14,e3=PICK_UP_E3}
 
 end
 
 function BELT_GRAB_POS:init()
-  self.args["jaco"] = {x=PROD_POS_X,y=PROD_POS_Y+PICK_UP_Y_OFFSET,z=PROD_POS_Z+0.02,
+  self.args["jaco"] = {x=PROD_POS_X+PICK_UP_X_OFFSET,y=PROD_POS_Y+PICK_UP_Y_OFFSET,z=PROD_POS_Z+0.02,
                         e1=1.57,e2=3.14,e3=PICK_UP_E3}
 end
 
 function RELAXED_BELT_GRAB_POS:init()
-  self.args["jaco"] = {x=PROD_POS_X,y=PROD_POS_Y+PICK_UP_Y_OFFSET,z=PROD_POS_Z+0.05,
+  self.args["jaco"] = {x=PROD_POS_X+PICK_UP_X_OFFSET,y=PROD_POS_Y+PICK_UP_Y_OFFSET,z=PROD_POS_Z+0.05,
                         e1=1.57,e2=3.14,e3=PICK_UP_E3}
 end
 
@@ -205,12 +209,17 @@ function CHECK_GRAB:init()
 end
 
 function MOVE_UP_FROM_BELT:init()
-  self.args["jaco"] = {x=PROD_POS_X,y=PROD_POS_Y+PICK_UP_Y_OFFSET,z=PROD_POS_Z+0.2,
+  self.args["jaco"] = {x=PROD_POS_X+PICK_UP_X_OFFSET,y=PROD_POS_Y+PICK_UP_Y_OFFSET,z=PROD_POS_Z+0.1,
+                        e1=1.57,e2=3.14,e3=PICK_UP_E3}
+end
+
+function MOVE_UP_FROM_BELT:init()
+  self.args["jaco"] = {x=PROD_POS_X+PICK_UP_X_OFFSET,y=PROD_POS_Y+PICK_UP_Y_OFFSET,z=PROD_POS_Z+0.05,
                         e1=1.57,e2=3.14,e3=PICK_UP_E3}
 end
 
 function MOVE_TO_MIDDLE:init()
-  self.args["jaco"] = {x=-0.1, y=0.4, z=0.95,e1=1.57,e2=3.14,e3=PICK_UP_E3}
+  self.args["jaco"] = {x=PARK_X, y=PARK_Y, z=PARK_Z,e1=1.57,e2=3.14,e3=PICK_UP_E3}
 end
 
 function HOVER_GROUND:init()

- *commit* ce751a17b138ca370d61cf2936b9c466d02476a2 - - - - - - - - - -
Author:  Till Hofmann <hofmann at kbsg.rwth-aachen.de>
Date:    Wed Jun 15 18:46:52 2016 +0200
Subject: add separators to limit the movement of the arm

 .../skills/generic/advanced/rcll_move_product.lua  |   23 ++++++++++++++++---
 1 files changed, 19 insertions(+), 4 deletions(-)

_Diff for modified files_:
diff --git a/src/lua/skills/generic/advanced/rcll_move_product.lua b/src/lua/skills/generic/advanced/rcll_move_product.lua
index 7c072f2..f33ecaa 100644
--- a/src/lua/skills/generic/advanced/rcll_move_product.lua
+++ b/src/lua/skills/generic/advanced/rcll_move_product.lua
@@ -61,8 +61,10 @@ fsm:define_states{ export_to=_M,
 
   {"START", SkillJumpState, final_to="ADD_OBJS", fail_to="FAILED",
     skills={{or_object}}},
-  -- add MPS and the product object
-  {"ADD_OBJS", SkillJumpState, final_to="MOVE_OBJS", fail_to="FAILED",
+  -- add MPS, the product object, and separators
+  {"ADD_OBJS", SkillJumpState, final_to="ROTATE_OBJS", fail_to="FAILED",
+    skills={{or_object}}},
+  {"ROTATE_OBJS", SkillJumpState, final_to="MOVE_OBJS", fail_to="FAILED",
     skills={{or_object}}},
   -- stack the product on top of the MPS belt
   {"MOVE_OBJS", SkillJumpState, final_to="INIT_PARK", fail_to="FAILED",
@@ -149,14 +151,27 @@ end
 function ADD_OBJS:init()
   self.args["or_object"] = {
     add={ {name="mps", path="rcll.machine.xml"},
-          {name="prod", path="rcll.prod.xml"}
+          {name="prod", path="rcll.prod.xml"},
+          {name="separator1", path="separator.kinbody.xml"},
+          {name="separator2", path="separator.kinbody.xml"},
+          {name="separator3", path="separator.kinbody.xml"}
           }}
 end
 
+function ROTATE_OBJS:init()
+  self.args["or_object"] = {
+    rotate={--{name="separator1",x=0,y=0,z=1.57}
+            {name="separator2",x=0,y=0,z=1.57}
+           }}
+end
+
 function MOVE_OBJS:init()
   self.args["or_object"] = {
     move={{name="mps", x=-0.10, y=0.47, z=0},
-          {name="prod", x=PROD_POS_X,y=PROD_POS_Y,z=PROD_POS_Z}
+          {name="prod", x=PROD_POS_X,y=PROD_POS_Y,z=PROD_POS_Z},
+          {name="separator1",x=0.0,y=-0.4,z=0.3},
+          {name="separator2",x=-0.2,y=0,z=0.3},
+          {name="separator3",x=0,y=0.5,z=0.3}
           }}
 end
 

- *commit* 6b1c0e1797e3ffdfed39e04e692d998e91741e5c - - - - - - - - - -
Author:  Till Hofmann <hofmann at kbsg.rwth-aachen.de>
Date:    Wed Jun 15 19:02:14 2016 +0200
Subject: add a box at the drop location

 .../skills/generic/advanced/rcll_move_product.lua  |    8 +++++---
 1 files changed, 5 insertions(+), 3 deletions(-)

_Diff for modified files_:
diff --git a/src/lua/skills/generic/advanced/rcll_move_product.lua b/src/lua/skills/generic/advanced/rcll_move_product.lua
index f33ecaa..7e5962d 100644
--- a/src/lua/skills/generic/advanced/rcll_move_product.lua
+++ b/src/lua/skills/generic/advanced/rcll_move_product.lua
@@ -54,7 +54,7 @@ local PICK_UP_X_OFFSET = -0.01
 local PICK_UP_Y_OFFSET = 0.00
 local DROP_POS_X = 0.2
 local DROP_POS_Y = -0.2
-local DROP_POS_Z = 0.1
+local DROP_POS_Z = 0.11
 
 -- States
 fsm:define_states{ export_to=_M,
@@ -154,7 +154,8 @@ function ADD_OBJS:init()
           {name="prod", path="rcll.prod.xml"},
           {name="separator1", path="separator.kinbody.xml"},
           {name="separator2", path="separator.kinbody.xml"},
-          {name="separator3", path="separator.kinbody.xml"}
+          {name="separator3", path="separator.kinbody.xml"},
+          {name="dropbox", path="dropbox.kinbody.xml"}
           }}
 end
 
@@ -171,7 +172,8 @@ function MOVE_OBJS:init()
           {name="prod", x=PROD_POS_X,y=PROD_POS_Y,z=PROD_POS_Z},
           {name="separator1",x=0.0,y=-0.4,z=0.3},
           {name="separator2",x=-0.2,y=0,z=0.3},
-          {name="separator3",x=0,y=0.5,z=0.3}
+          {name="separator3",x=0,y=0.5,z=0.3},
+          {name="dropbox", x=0.2,y=-0.2,z=0}
           }}
 end
 

- *commit* 46fc7a808dc998e0504fb824e850f98dead40402 - - - - - - - - - -
Author:  Till Hofmann <hofmann at kbsg.rwth-aachen.de>
Date:    Thu Jun 23 17:52:20 2016 +0200
Subject: add model for a simple box where the product is placed

 .../{plate.kinbody.xml => dropbox.kinbody.xml}     |    8 ++++----
 1 files changed, 4 insertions(+), 4 deletions(-)





-- 
Fawkes Robotics Framework                 http://www.fawkesrobotics.org


More information about the fawkes-commits mailing list