Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[7141] pkg/trunk
|
From: <rdi...@us...> - 2008-11-21 18:26:14
|
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.
|
