Thread: [Personalrobots-commit] SF.net SVN: personalrobots:[7141] pkg/trunk (Page 17)

SourceForge Headquarters 1320 Columbia Street Suite 310 San Diego, CA 92101 +1 (858) 422-6466

Revision: 7141
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=7141&view=rev
Author:   rdiankov
Date:     2008-11-21 18:26:09 +0000 (Fri, 21 Nov 2008)

Log Message:
-----------
fixed openrave robot issues

Modified Paths:
--------------
    pkg/trunk/3rdparty/openrave/Makefile
    pkg/trunk/openrave_planning/or_rosplanning/src/rosarmik.cpp
    pkg/trunk/openrave_planning/or_rosplanning/test/testrosik.m
    pkg/trunk/robot_descriptions/openrave_robot_description/CMakeLists.txt
    pkg/trunk/robot_descriptions/openrave_robot_description/robots/pr2full.robot.xml
    pkg/trunk/robot_descriptions/openrave_robot_description/src/urdf2openrave.cpp
    pkg/trunk/robot_descriptions/openrave_robot_description/test/test_robots.cpp

Modified: pkg/trunk/3rdparty/openrave/Makefile
===================================================================

--- pkg/trunk/3rdparty/openrave/Makefile	2008-11-21 18:09:26 UTC (rev 7140)
+++ pkg/trunk/3rdparty/openrave/Makefile	2008-11-21 18:26:09 UTC (rev 7141)
@@ -2,7 +2,7 @@
 
 SVN_DIR = openrave_svn
 # Should really specify a revision
-SVN_REVISION = -r 499
+SVN_REVISION = -r 500
 SVN_URL = https://openrave.svn.sourceforge.net/svnroot/openrave
 include $(shell rospack find mk)/svn_checkout.mk
 

Modified: pkg/trunk/openrave_planning/or_rosplanning/src/rosarmik.cpp
===================================================================
--- pkg/trunk/openrave_planning/or_rosplanning/src/rosarmik.cpp	2008-11-21 18:09:26 UTC (rev 7140)
+++ pkg/trunk/openrave_planning/or_rosplanning/src/rosarmik.cpp	2008-11-21 18:26:09 UTC (rev 7141)
@@ -72,49 +72,49 @@
     axis.push_back(aj);
     an << 0 << 0 << 0;
     anchor.push_back(an);
-    _vjointmult.push_back(-1);
+    _vjointmult.push_back(1);
 
     // Shoulder pitch
     aj << 0 << 1 << 0;
     axis.push_back(aj);
     an << 0.1 << 0 << 0;
     anchor.push_back(an);
-    _vjointmult.push_back(-1);
+    _vjointmult.push_back(1);
 
     // Shoulder roll
     aj << 1 << 0 << 0;
     axis.push_back(aj);
     an << 0.1 << 0 << 0;
     anchor.push_back(an);
-    _vjointmult.push_back(-1);
+    _vjointmult.push_back(1);
 
     // Elbow flex
     aj << 0 << 1 << 0;
     axis.push_back(aj);
     an << 0.5 << 0 << 0;
     anchor.push_back(an);
-    _vjointmult.push_back(1);
+    _vjointmult.push_back(-1);
 
     // Forearm roll
     aj << 1 << 0 << 0;
     axis.push_back(aj);
     an << 0.5 << 0 << 0;
     anchor.push_back(an);
-    _vjointmult.push_back(1);
+    _vjointmult.push_back(-1);
 
     // Wrist flex
     aj << 0 << 1 << 0;
     axis.push_back(aj);
     an << 0.82025 << 0 << 0;
     anchor.push_back(an);
-    _vjointmult.push_back(1);
+    _vjointmult.push_back(-1);
 
     // Gripper roll
     aj << 1 << 0 << 0;
     axis.push_back(aj);
     an << 0.82025 << 0 << 0;
     anchor.push_back(an);
-    _vjointmult.push_back(-1);
+    _vjointmult.push_back(1);
 
     for(int i=0; i < 7; i++)
         joint_type[i] = std::string("ROTARY");

Modified: pkg/trunk/openrave_planning/or_rosplanning/test/testrosik.m
===================================================================
--- pkg/trunk/openrave_planning/or_rosplanning/test/testrosik.m	2008-11-21 18:09:26 UTC (rev 7140)
+++ pkg/trunk/openrave_planning/or_rosplanning/test/testrosik.m	2008-11-21 18:26:09 UTC (rev 7141)
@@ -8,7 +8,8 @@
 
 manips = orRobotGetManipulators(robotid);
 
-% orBodySetJointValues(robotid,[ -0.21 0.754256 0.40712 0.323661 1.28898 0.175237 2.13528],manips{1}.armjoints);
+%% test any specific ik configuration
+% orBodySetJointValues(robotid,[ -0.503115 0.573333 -2.99203 0.365955 1.95347 2.04003 -0.175888 ],manips{1}.armjoints);
 % links = orBodyGetLinks(robotid);
 % Thand = reshape(links(:,manips{1}.eelink),[3 4]);
 % s = orProblemSendCommand(['iktest matrix ' sprintf('%f ',Thand(:))]);

Modified: pkg/trunk/robot_descriptions/openrave_robot_description/CMakeLists.txt
===================================================================
--- pkg/trunk/robot_descriptions/openrave_robot_description/CMakeLists.txt	2008-11-21 18:09:26 UTC (rev 7140)
+++ pkg/trunk/robot_descriptions/openrave_robot_description/CMakeLists.txt	2008-11-21 18:26:09 UTC (rev 7141)
@@ -26,4 +26,7 @@
 
 # add the test package
 rospack_add_gtest(test_or_robots test/test_robots.cpp)
-set_target_properties(test_or_robots PROPERTIES COMPILE_FLAGS "-DROBOT_FILES=\\\"${robot_files}\\\" -DOPENRAVE_EXECUTABLE=\\\"${openrave_PACKAGE_PATH}/bin/openrave\\\"")
+
+string(REGEX REPLACE ";" ":" robot_test_files "${robot_files}")
+set(robot_test_files "${robot_test_files}:${openrave_robot_description_PACKAGE_PATH}/robots/pr2full.robot.xml")
+set_target_properties(test_or_robots PROPERTIES COMPILE_FLAGS "-DROBOT_FILES=\\\"${robot_test_files}\\\" -DOPENRAVE_EXECUTABLE=\\\"${openrave_PACKAGE_PATH}/bin/openrave\\\"")

Modified: pkg/trunk/robot_descriptions/openrave_robot_description/robots/pr2full.robot.xml
===================================================================
--- pkg/trunk/robot_descriptions/openrave_robot_description/robots/pr2full.robot.xml	2008-11-21 18:09:26 UTC (rev 7140)
+++ pkg/trunk/robot_descriptions/openrave_robot_description/robots/pr2full.robot.xml	2008-11-21 18:26:09 UTC (rev 7141)
@@ -2,10 +2,10 @@
 <Robot file="pr2.robot.xml">
   <!-- left gripper -->
   <Manipulator>
-    <base>torso_lift</base>
-    <effector>l_gripper_palm</effector>
-    <armjoints>l_shoulder_pan_joint l_shoulder_lift_joint l_upper_arm_roll_joint l_elbow_flex_joint l_forearm_roll_joint l_wrist_flex_joint l_wrist_roll_joint</armjoints>
-    <joints>l_gripper_l_finger_joint</joints>
+    <base>torso</base>
+    <effector>left_gripper_palm</effector>
+    <armjoints>left_shoulder_pan_joint left_shoulder_pitch_joint left_upper_arm_roll_joint left_elbow_flex_joint left_forearm_roll_joint left_wrist_flex_joint left_wrist_roll_joint</armjoints>
+    <joints>left_gripper_l_finger_joint</joints>
     <closed>0</closed>
     <opened>0.8</opened>
     <iksolver>rosarmik</iksolver>
@@ -13,10 +13,10 @@
 
   <!-- right gripper -->
   <Manipulator>
-    <base>torso_lift</base>
-    <effector>r_gripper_palm</effector>
-    <armjoints>r_shoulder_pan_joint r_shoulder_lift_joint r_upper_arm_roll_joint r_elbow_flex_joint r_forearm_roll_joint r_wrist_flex_joint r_wrist_roll_joint</armjoints>
-    <joints>r_gripper_l_finger_joint</joints>
+    <base>torso</base>
+    <effector>right_gripper_palm</effector>
+    <armjoints>right_shoulder_pan_joint right_shoulder_pitch_joint right_upper_arm_roll_joint right_elbow_flex_joint right_forearm_roll_joint right_wrist_flex_joint right_wrist_roll_joint</armjoints>
+    <joints>right_gripper_l_finger_joint</joints>
     <closed>0</closed>
     <opened>0.8</opened>
     <iksolver>rosarmik</iksolver>

Modified: pkg/trunk/robot_descriptions/openrave_robot_description/src/urdf2openrave.cpp
===================================================================
--- pkg/trunk/robot_descriptions/openrave_robot_description/src/urdf2openrave.cpp	2008-11-21 18:09:26 UTC (rev 7140)
+++ pkg/trunk/robot_descriptions/openrave_robot_description/src/urdf2openrave.cpp	2008-11-21 18:26:09 UTC (rev 7141)
@@ -90,13 +90,12 @@
 {
     btVector3 pz = transform.getOrigin();
     double cpos[3] = { pz.x(), pz.y(), pz.z() };
-    btMatrix3x3 mat = transform.getBasis();
-    double crot[3];
-    mat.getEulerZYX(crot[2],crot[1],crot[0]);
+    btQuaternion qt = transform.getRotation();
+    double cquat[4] = { qt.getW(),qt.getX(), qt.getY(), qt.getZ() };
     
-    /* set geometry transform */
-    addKeyValue(elem, "xyz", values2str(3, cpos));
-    addKeyValue(elem, "rpy", values2str(3, crot, rad2deg));  
+    // set geometry transform
+    addKeyValue(elem, "translation", values2str(3, cpos));
+    addKeyValue(elem, "quaternion", values2str(4, cquat));  
 }
 
 void copyOpenraveMap(const robot_desc::URDF::Map& data, TiXmlElement *elem, const vector<string> *tags = NULL)
@@ -277,8 +276,9 @@
             addKeyValue(joint, "highStop", "0");
             addKeyValue(joint, "axis", "1 0 0");
         }
-        else {        
-            addKeyValue(joint, "axis", values2str(3, link->joint->axis));
+        else {
+            double jaxis[3] = {-link->joint->axis[0], -link->joint->axis[1], -link->joint->axis[2]};
+            addKeyValue(joint, "axis", values2str(3, jaxis));
             addKeyValue(joint, "anchor", values2str(3, link->joint->anchor));
 		
             if (link->joint->pjointMimic == NULL  && enforce_limits && link->joint->isSet["limit"]) {

Modified: pkg/trunk/robot_descriptions/openrave_robot_description/test/test_robots.cpp
===================================================================
--- pkg/trunk/robot_descriptions/openrave_robot_description/test/test_robots.cpp	2008-11-21 18:09:26 UTC (rev 7140)
+++ pkg/trunk/robot_descriptions/openrave_robot_description/test/test_robots.cpp	2008-11-21 18:26:09 UTC (rev 7141)
@@ -28,7 +28,7 @@
 TEST(URDF, LoadRobots)
 {
     bool bSuccess = true;
-    vector<string> files = tokenizer(ROBOT_FILES,"; \r\n");
+    vector<string> files = tokenizer(ROBOT_FILES,":; \r\n");
     for(vector<string>::iterator it = files.begin(); it != files.end(); ++it) {
         cout << "testing: " << *it << "... ";
         int result = runExternalProcess(OPENRAVE_EXECUTABLE, *it + string(" -testscene -nogui"));


This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.




Thread: [Personalrobots-commit] SF.net SVN: personalrobots:[7141] pkg/trunk (Page 17)

personalrobots-commit