Menu
▾
▴
personalrobots-commit
|
From: <ge...@us...> - 2008-07-01 22:54:52
|
Revision: 1166
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=1166&view=rev
Author: gerkey
Date: 2008-07-01 15:55:00 -0700 (Tue, 01 Jul 2008)
Log Message:
-----------
moved all existing CMake builds to new rospack macro usage
Modified Paths:
--------------
pkg/trunk/3rdparty/drivers/sicktoolbox/manifest.xml
pkg/trunk/drivers/cam/ptgrey/bumblebee_bridge/CMakeLists.txt
pkg/trunk/drivers/cam/ptgrey/bumblebee_bridge/Makefile
pkg/trunk/drivers/laser/sicktoolbox_wrapper/CMakeLists.txt
pkg/trunk/drivers/laser/sicktoolbox_wrapper/Makefile
pkg/trunk/drivers/laser/sicktoolbox_wrapper/ros/sicklms/CMakeLists.txt
pkg/trunk/drivers/laser/sicktoolbox_wrapper/standalone/print_scans/CMakeLists.txt
pkg/trunk/nav/deadreckon/CMakeLists.txt
pkg/trunk/nav/deadreckon/Makefile
pkg/trunk/nav/map_server/CMakeLists.txt
pkg/trunk/nav/map_server/Makefile
pkg/trunk/nav/wavefront_player/CMakeLists.txt
pkg/trunk/nav/wavefront_player/Makefile
pkg/trunk/util/logsetta/CMakeLists.txt
pkg/trunk/util/logsetta/Makefile
pkg/trunk/util/logsetta/carmen/genlog_carmen/CMakeLists.txt
pkg/trunk/util/mux/CMakeLists.txt
pkg/trunk/util/mux/Makefile
pkg/trunk/vision/camera_grab_samples/bumblebee_grab_sample/CMakeLists.txt
pkg/trunk/vision/camera_grab_samples/bumblebee_grab_sample/Makefile
Modified: pkg/trunk/3rdparty/drivers/sicktoolbox/manifest.xml
===================================================================
--- pkg/trunk/3rdparty/drivers/sicktoolbox/manifest.xml 2008-07-01 22:46:43 UTC (rev 1165)
+++ pkg/trunk/3rdparty/drivers/sicktoolbox/manifest.xml 2008-07-01 22:55:00 UTC (rev 1166)
@@ -8,7 +8,7 @@
<license>BSD</license>
<url>http://sicktoolbox.sourceforge.net</url>
<export>
- <cpp lflags="-rpath ${prefix}/lib -L${prefix}/sicktoolbox/lib -lsicklms-1.0" cflags="-I${prefix}/sicktoolbox/include"/>
+ <cpp lflags="-Wl,-rpath,${prefix}/sicktoolbox/lib -L${prefix}/sicktoolbox/lib -lsicklms-1.0" cflags="-I${prefix}/sicktoolbox/include"/>
</export>
</package>
Modified: pkg/trunk/drivers/cam/ptgrey/bumblebee_bridge/CMakeLists.txt
===================================================================
--- pkg/trunk/drivers/cam/ptgrey/bumblebee_bridge/CMakeLists.txt 2008-07-01 22:46:43 UTC (rev 1165)
+++ pkg/trunk/drivers/cam/ptgrey/bumblebee_bridge/CMakeLists.txt 2008-07-01 22:55:00 UTC (rev 1166)
@@ -1,8 +1,4 @@
cmake_minimum_required(VERSION 2.6)
-set(CMAKE_MODULE_PATH $ENV{ROS_ROOT}/core/rosbuild)
include(rosbuild)
-project(bumblebee_bridge)
-rospack()
-set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/)
-add_executable(bumblebee_bridge bumblebee_bridge.cpp)
-target_link_libraries(bumblebee_bridge ${bumblebee_bridge_LIBRARIES})
+rospack(bumblebee_bridge)
+rospack_add_executable(bumblebee_bridge bumblebee_bridge.cpp)
Modified: pkg/trunk/drivers/cam/ptgrey/bumblebee_bridge/Makefile
===================================================================
--- pkg/trunk/drivers/cam/ptgrey/bumblebee_bridge/Makefile 2008-07-01 22:46:43 UTC (rev 1165)
+++ pkg/trunk/drivers/cam/ptgrey/bumblebee_bridge/Makefile 2008-07-01 22:55:00 UTC (rev 1166)
@@ -1,8 +1,2 @@
-all: build/Makefile
- cd build && make -s
-build/Makefile:
- mkdir -p build
- cd build && cmake ..
-clean:
- if [ -f build/Makefile ]; then cd build && make clean && cd ..; fi
- rm -rf build
+include $(shell rospack find mk)/cmake.mk
+
Modified: pkg/trunk/drivers/laser/sicktoolbox_wrapper/CMakeLists.txt
===================================================================
--- pkg/trunk/drivers/laser/sicktoolbox_wrapper/CMakeLists.txt 2008-07-01 22:46:43 UTC (rev 1165)
+++ pkg/trunk/drivers/laser/sicktoolbox_wrapper/CMakeLists.txt 2008-07-01 22:55:00 UTC (rev 1166)
@@ -1,7 +1,5 @@
cmake_minimum_required(VERSION 2.6)
-set(CMAKE_MODULE_PATH $ENV{ROS_ROOT}/core/rosbuild)
include(rosbuild)
-project(sicktoolbox_wrapper)
-rospack()
+rospack(sicktoolbox_wrapper)
set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
subdirs(standalone ros)
Modified: pkg/trunk/drivers/laser/sicktoolbox_wrapper/Makefile
===================================================================
--- pkg/trunk/drivers/laser/sicktoolbox_wrapper/Makefile 2008-07-01 22:46:43 UTC (rev 1165)
+++ pkg/trunk/drivers/laser/sicktoolbox_wrapper/Makefile 2008-07-01 22:55:00 UTC (rev 1166)
@@ -1,8 +1 @@
-all: build/Makefile
- cd build && make -s
-build/Makefile:
- mkdir -p build
- cd build && cmake ..
-clean:
- if [ -f build/Makefile ]; then cd build && make clean && cd ..; fi
- rm -rf build
+include $(shell rospack find mk)/cmake.mk
Modified: pkg/trunk/drivers/laser/sicktoolbox_wrapper/ros/sicklms/CMakeLists.txt
===================================================================
--- pkg/trunk/drivers/laser/sicktoolbox_wrapper/ros/sicklms/CMakeLists.txt 2008-07-01 22:46:43 UTC (rev 1165)
+++ pkg/trunk/drivers/laser/sicktoolbox_wrapper/ros/sicklms/CMakeLists.txt 2008-07-01 22:55:00 UTC (rev 1166)
@@ -1,2 +1 @@
-add_executable(sicklms sicklms.cpp)
-target_link_libraries(sicklms ${sicktoolbox_wrapper_LIBRARIES})
+rospack_add_executable(sicklms sicklms.cpp)
Modified: pkg/trunk/drivers/laser/sicktoolbox_wrapper/standalone/print_scans/CMakeLists.txt
===================================================================
--- pkg/trunk/drivers/laser/sicktoolbox_wrapper/standalone/print_scans/CMakeLists.txt 2008-07-01 22:46:43 UTC (rev 1165)
+++ pkg/trunk/drivers/laser/sicktoolbox_wrapper/standalone/print_scans/CMakeLists.txt 2008-07-01 22:55:00 UTC (rev 1166)
@@ -1,2 +1,2 @@
-add_executable(print_scans print_scans.cpp)
-target_link_libraries(print_scans ${sicktoolbox_wrapper_LIBRARIES} pthread)
+rospack_add_executable(print_scans print_scans.cpp)
+target_link_libraries(pthread)
Modified: pkg/trunk/nav/deadreckon/CMakeLists.txt
===================================================================
--- pkg/trunk/nav/deadreckon/CMakeLists.txt 2008-07-01 22:46:43 UTC (rev 1165)
+++ pkg/trunk/nav/deadreckon/CMakeLists.txt 2008-07-01 22:55:00 UTC (rev 1166)
@@ -1,10 +1,7 @@
cmake_minimum_required(VERSION 2.6)
-set(CMAKE_MODULE_PATH $ENV{ROS_ROOT}/core/rosbuild)
include(rosbuild)
-project(deadreckon)
-rospack()
+rospack(deadreckon)
gensrv()
-set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR})
# the service provider
rospack_add_executable(deadreckon deadreckon.cpp)
# a dumb test client
Modified: pkg/trunk/nav/deadreckon/Makefile
===================================================================
--- pkg/trunk/nav/deadreckon/Makefile 2008-07-01 22:46:43 UTC (rev 1165)
+++ pkg/trunk/nav/deadreckon/Makefile 2008-07-01 22:55:00 UTC (rev 1166)
@@ -1,8 +1,2 @@
-all: build/Makefile
- cd build && make -s
-build/Makefile:
- mkdir -p build
- cd build && cmake ..
-clean:
- if [ -f build/Makefile ]; then cd build && make clean && cd ..; fi
- rm -rf build
+include $(shell rospack find mk)/cmake.mk
+
Modified: pkg/trunk/nav/map_server/CMakeLists.txt
===================================================================
--- pkg/trunk/nav/map_server/CMakeLists.txt 2008-07-01 22:46:43 UTC (rev 1165)
+++ pkg/trunk/nav/map_server/CMakeLists.txt 2008-07-01 22:55:00 UTC (rev 1166)
@@ -1,9 +1,6 @@
cmake_minimum_required(VERSION 2.6 FATAL_ERROR)
-set(CMAKE_MODULE_PATH $ENV{ROS_ROOT}/core/rosbuild)
-set(EXECUTABLE_OUTPUT_PATH ..)
include(rosbuild)
-project(map_server)
-rospack()
+rospack(map_server)
rospack_add_executable(map_server map_server.cc)
Modified: pkg/trunk/nav/map_server/Makefile
===================================================================
--- pkg/trunk/nav/map_server/Makefile 2008-07-01 22:46:43 UTC (rev 1165)
+++ pkg/trunk/nav/map_server/Makefile 2008-07-01 22:55:00 UTC (rev 1166)
@@ -1,15 +1 @@
-#SRC = map_server.cc
-#OUT = map_server
-#PKG = map_server
-#CFLAGS = -Wall -Werror `pkg-config --cflags gdk-pixbuf-2.0`
-#LFLAGS = `pkg-config --libs gdk-pixbuf-2.0`
-#include $(shell rospack find mk)/node.mk
-
-all: build/Makefile
- cd build && make -s
-build/Makefile:
- mkdir -p build
- cd build && cmake -DCMAKE_BUILD_TYPE=Debug ..
-clean:
- if [ -f build/Makefile ]; then cd build && make clean && cd ..; fi
- rm -rf build
+include $(shell rospack find mk)/cmake.mk
Modified: pkg/trunk/nav/wavefront_player/CMakeLists.txt
===================================================================
--- pkg/trunk/nav/wavefront_player/CMakeLists.txt 2008-07-01 22:46:43 UTC (rev 1165)
+++ pkg/trunk/nav/wavefront_player/CMakeLists.txt 2008-07-01 22:55:00 UTC (rev 1166)
@@ -1,15 +1,11 @@
cmake_minimum_required(VERSION 2.6)
-set(CMAKE_MODULE_PATH $ENV{ROS_ROOT}/core/rosbuild)
include(rosbuild)
-project(wavefront_player)
-rospack()
+rospack(wavefront_player)
gensrv()
-set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR})
# the service provider
rospack_add_executable(wavefront_player wavefront_player.cc)
-target_link_libraries(wavefront_player ${wavefront_player_LIBRARIES} wavefront_standalone)
+target_link_libraries(wavefront_player wavefront_standalone)
+
rospack_add_executable(send_goal send_goal.cc)
-target_link_libraries(send_goal ${wavefront_player_LIBRARIES})
rospack_add_executable(query_wavefront query_wavefront.cpp)
-target_link_libraries(query_wavefront ${wavefront_player_LIBRARIES})
Modified: pkg/trunk/nav/wavefront_player/Makefile
===================================================================
--- pkg/trunk/nav/wavefront_player/Makefile 2008-07-01 22:46:43 UTC (rev 1165)
+++ pkg/trunk/nav/wavefront_player/Makefile 2008-07-01 22:55:00 UTC (rev 1166)
@@ -1,8 +1 @@
-all: build/Makefile
- cd build && make -s
-build/Makefile:
- mkdir -p build
- cd build && cmake ..
-clean:
- if [ -f build/Makefile ]; then cd build && make clean && cd ..; fi
- rm -rf build
+include $(shell rospack find mk)/cmake.mk
Modified: pkg/trunk/util/logsetta/CMakeLists.txt
===================================================================
--- pkg/trunk/util/logsetta/CMakeLists.txt 2008-07-01 22:46:43 UTC (rev 1165)
+++ pkg/trunk/util/logsetta/CMakeLists.txt 2008-07-01 22:55:00 UTC (rev 1166)
@@ -1,7 +1,5 @@
cmake_minimum_required(VERSION 2.6)
-set(CMAKE_MODULE_PATH $ENV{ROS_ROOT}/core/rosbuild)
include(rosbuild)
-project(logsetta)
-rospack()
+rospack(logsetta)
set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
subdirs(carmen)
Modified: pkg/trunk/util/logsetta/Makefile
===================================================================
--- pkg/trunk/util/logsetta/Makefile 2008-07-01 22:46:43 UTC (rev 1165)
+++ pkg/trunk/util/logsetta/Makefile 2008-07-01 22:55:00 UTC (rev 1166)
@@ -1,8 +1 @@
-all: build/Makefile
- cd build && make -s
-build/Makefile:
- mkdir -p build
- cd build && cmake ..
-clean:
- if [ -f build/Makefile ]; then cd build && make clean && cd ..; fi
- rm -rf build bin
+include $(shell rospack find mk)/cmake.mk
Modified: pkg/trunk/util/logsetta/carmen/genlog_carmen/CMakeLists.txt
===================================================================
--- pkg/trunk/util/logsetta/carmen/genlog_carmen/CMakeLists.txt 2008-07-01 22:46:43 UTC (rev 1165)
+++ pkg/trunk/util/logsetta/carmen/genlog_carmen/CMakeLists.txt 2008-07-01 22:55:00 UTC (rev 1166)
@@ -1,2 +1 @@
-add_executable(genlog_carmen genlog_carmen.cpp)
-target_link_libraries(genlog_carmen ${logsetta_LIBRARIES})
+rospack_add_executable(genlog_carmen genlog_carmen.cpp)
Modified: pkg/trunk/util/mux/CMakeLists.txt
===================================================================
--- pkg/trunk/util/mux/CMakeLists.txt 2008-07-01 22:46:43 UTC (rev 1165)
+++ pkg/trunk/util/mux/CMakeLists.txt 2008-07-01 22:55:00 UTC (rev 1166)
@@ -1,8 +1,4 @@
cmake_minimum_required(VERSION 2.6)
-set(CMAKE_MODULE_PATH $ENV{ROS_ROOT}/core/rosbuild)
include(rosbuild)
-project(mux)
-rospack()
-set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR})
-add_executable(mux mux.cpp)
-target_link_libraries(mux ${mux_LIBRARIES})
+rospack(mux)
+rospack_add_executable(mux mux.cpp)
Modified: pkg/trunk/util/mux/Makefile
===================================================================
--- pkg/trunk/util/mux/Makefile 2008-07-01 22:46:43 UTC (rev 1165)
+++ pkg/trunk/util/mux/Makefile 2008-07-01 22:55:00 UTC (rev 1166)
@@ -1,8 +1 @@
-all: build/Makefile
- cd build && make -s
-build/Makefile:
- mkdir -p build
- cd build && cmake ..
-clean:
- if [ -f build/Makefile ]; then cd build && make clean && cd ..; fi
- rm -rf build
+include $(shell rospack find mk)/cmake.mk
Modified: pkg/trunk/vision/camera_grab_samples/bumblebee_grab_sample/CMakeLists.txt
===================================================================
--- pkg/trunk/vision/camera_grab_samples/bumblebee_grab_sample/CMakeLists.txt 2008-07-01 22:46:43 UTC (rev 1165)
+++ pkg/trunk/vision/camera_grab_samples/bumblebee_grab_sample/CMakeLists.txt 2008-07-01 22:55:00 UTC (rev 1166)
@@ -1,8 +1,4 @@
cmake_minimum_required(VERSION 2.6)
-set(CMAKE_MODULE_PATH $ENV{ROS_ROOT}/core/rosbuild)
include(rosbuild)
-project(bumblebee_grab_sample)
-rospack()
-set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/)
-add_executable(bumblebee_grab_sample bumblebee_grab_sample.cpp)
-target_link_libraries(bumblebee_grab_sample ${bumblebee_grab_sample_LIBRARIES})
+rospack(bumblebee_grab_sample)
+rospack_add_executable(bumblebee_grab_sample bumblebee_grab_sample.cpp)
Modified: pkg/trunk/vision/camera_grab_samples/bumblebee_grab_sample/Makefile
===================================================================
--- pkg/trunk/vision/camera_grab_samples/bumblebee_grab_sample/Makefile 2008-07-01 22:46:43 UTC (rev 1165)
+++ pkg/trunk/vision/camera_grab_samples/bumblebee_grab_sample/Makefile 2008-07-01 22:55:00 UTC (rev 1166)
@@ -1,8 +1 @@
-all: build/Makefile
- cd build && make -s
-build/Makefile:
- mkdir -p build
- cd build && cmake ..
-clean:
- if [ -f build/Makefile ]; then cd build && make clean && cd ..; fi
- rm -rf build
+include $(shell rospack find mk)/cmake.mk
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <hsu...@us...> - 2008-07-02 02:43:51
|
Revision: 1188
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=1188&view=rev
Author: hsujohnhsu
Date: 2008-07-01 19:43:58 -0700 (Tue, 01 Jul 2008)
Log Message:
-----------
made robot more stable by specifying damping.
Modified Paths:
--------------
pkg/trunk/3rdparty/gazebo/gazebo_patch.diff
pkg/trunk/demos/2dnav-gazebo/world/pr2.model
pkg/trunk/demos/2dnav-gazebo/world/robot.world
Modified: pkg/trunk/3rdparty/gazebo/gazebo_patch.diff
===================================================================
--- pkg/trunk/3rdparty/gazebo/gazebo_patch.diff 2008-07-02 02:17:07 UTC (rev 1187)
+++ pkg/trunk/3rdparty/gazebo/gazebo_patch.diff 2008-07-02 02:43:58 UTC (rev 1188)
@@ -1,6 +1,6 @@
Index: gazebo-svn/player/SConscript
===================================================================
---- gazebo-svn/player/SConscript (revision 6715)
+--- gazebo-svn/player/SConscript (revision 6696)
+++ gazebo-svn/player/SConscript (working copy)
@@ -1,7 +1,8 @@
import os
@@ -14,7 +14,7 @@
'GazeboClient.cc',
Index: gazebo-svn/libgazebo/Server.cc
===================================================================
---- gazebo-svn/libgazebo/Server.cc (revision 6715)
+--- gazebo-svn/libgazebo/Server.cc (revision 6696)
+++ gazebo-svn/libgazebo/Server.cc (working copy)
@@ -38,6 +38,7 @@
#include <sys/sem.h>
@@ -103,7 +103,7 @@
{
Index: gazebo-svn/libgazebo/Iface.cc
===================================================================
---- gazebo-svn/libgazebo/Iface.cc (revision 6715)
+--- gazebo-svn/libgazebo/Iface.cc (revision 6696)
+++ gazebo-svn/libgazebo/Iface.cc (working copy)
@@ -55,6 +55,8 @@
GZ_REGISTER_IFACE("factory", FactoryIface);
@@ -116,7 +116,7 @@
Index: gazebo-svn/libgazebo/gazebo.h
===================================================================
---- gazebo-svn/libgazebo/gazebo.h (revision 6715)
+--- gazebo-svn/libgazebo/gazebo.h (revision 6696)
+++ gazebo-svn/libgazebo/gazebo.h (working copy)
@@ -550,7 +550,7 @@
@@ -604,7 +604,7 @@
};
Index: gazebo-svn/server/physics/HingeJoint.cc
===================================================================
---- gazebo-svn/server/physics/HingeJoint.cc (revision 6715)
+--- gazebo-svn/server/physics/HingeJoint.cc (revision 6696)
+++ gazebo-svn/server/physics/HingeJoint.cc (working copy)
@@ -38,6 +38,7 @@
: Joint()
@@ -614,9 +614,29 @@
}
+Index: gazebo-svn/server/physics/ode/ODEPhysics.cc
+===================================================================
+--- gazebo-svn/server/physics/ode/ODEPhysics.cc (revision 6696)
++++ gazebo-svn/server/physics/ode/ODEPhysics.cc (working copy)
+@@ -266,13 +266,12 @@
+ // Compute the CFM and ERP by assuming the two bodies form a
+ // spring-damper system.
+ h = self->stepTime;
+- kp = 1 / (1 / geom1->contact->kp + 1 / geom2->contact->kp);
++ kp = 1.0 / (1.0 / geom1->contact->kp + 1.0 / geom2->contact->kp);
+ kd = geom1->contact->kd + geom2->contact->kd;
+ contact.surface.mode |= dContactSoftERP | dContactSoftCFM;
+ contact.surface.soft_erp = h * kp / (h * kp + kd);
+- contact.surface.soft_cfm = 1 / (h * kp + kd);
++ contact.surface.soft_cfm = 1.0 / (h * kp + kd);
+
+-
+ //contacts[i].surface.mode |= dContactBounce | dContactSoftCFM;
+ contact.surface.mode |= dContactApprox1;
+ contact.surface.mu = MIN(geom1->contact->mu1, geom2->contact->mu1);
Index: gazebo-svn/server/physics/SliderJoint.cc
===================================================================
---- gazebo-svn/server/physics/SliderJoint.cc (revision 6715)
+--- gazebo-svn/server/physics/SliderJoint.cc (revision 6696)
+++ gazebo-svn/server/physics/SliderJoint.cc (working copy)
@@ -35,6 +35,8 @@
: Joint()
@@ -629,7 +649,7 @@
Index: gazebo-svn/server/sensors/Sensor.hh
===================================================================
---- gazebo-svn/server/sensors/Sensor.hh (revision 6715)
+--- gazebo-svn/server/sensors/Sensor.hh (revision 6696)
+++ gazebo-svn/server/sensors/Sensor.hh (working copy)
@@ -63,6 +63,7 @@
@@ -657,7 +677,7 @@
#endif
Index: gazebo-svn/server/sensors/camera/MonoCameraSensor.cc
===================================================================
---- gazebo-svn/server/sensors/camera/MonoCameraSensor.cc (revision 6715)
+--- gazebo-svn/server/sensors/camera/MonoCameraSensor.cc (revision 6696)
+++ gazebo-svn/server/sensors/camera/MonoCameraSensor.cc (working copy)
@@ -49,6 +49,7 @@
MonoCameraSensor::MonoCameraSensor(Body *body)
@@ -748,7 +768,7 @@
////////////////////////////////////////////////////////////////////////////////
Index: gazebo-svn/server/sensors/camera/StereoCameraSensor.cc
===================================================================
---- gazebo-svn/server/sensors/camera/StereoCameraSensor.cc (revision 6715)
+--- gazebo-svn/server/sensors/camera/StereoCameraSensor.cc (revision 6696)
+++ gazebo-svn/server/sensors/camera/StereoCameraSensor.cc (working copy)
@@ -58,6 +58,7 @@
this->depthBuffer[1] = NULL;
@@ -913,7 +933,7 @@
////////////////////////////////////////////////////////////////////////////////
Index: gazebo-svn/server/sensors/ray/RaySensor.cc
===================================================================
---- gazebo-svn/server/sensors/ray/RaySensor.cc (revision 6715)
+--- gazebo-svn/server/sensors/ray/RaySensor.cc (revision 6696)
+++ gazebo-svn/server/sensors/ray/RaySensor.cc (working copy)
@@ -237,7 +237,7 @@
// Update the sensor information
@@ -926,7 +946,7 @@
Pose3d poseDelta;
Index: gazebo-svn/server/sensors/Sensor.cc
===================================================================
---- gazebo-svn/server/sensors/Sensor.cc (revision 6715)
+--- gazebo-svn/server/sensors/Sensor.cc (revision 6696)
+++ gazebo-svn/server/sensors/Sensor.cc (working copy)
@@ -32,6 +32,7 @@
#include "World.hh"
@@ -975,7 +995,7 @@
+
Index: gazebo-svn/server/GazeboConfig.cc
===================================================================
---- gazebo-svn/server/GazeboConfig.cc (revision 6715)
+--- gazebo-svn/server/GazeboConfig.cc (revision 6696)
+++ gazebo-svn/server/GazeboConfig.cc (working copy)
@@ -56,6 +56,17 @@
@@ -997,7 +1017,7 @@
XMLConfig rc;
Index: gazebo-svn/server/gui/GLWindow.cc
===================================================================
---- gazebo-svn/server/gui/GLWindow.cc (revision 6715)
+--- gazebo-svn/server/gui/GLWindow.cc (revision 6696)
+++ gazebo-svn/server/gui/GLWindow.cc (working copy)
@@ -66,7 +66,11 @@
this->directionVec.x = 0;
@@ -1011,7 +1031,18 @@
this->keys.clear();
if (activeWin == NULL)
-@@ -235,6 +239,21 @@
+@@ -91,8 +95,9 @@
+ this->userCamera = new UserCamera( this );
+ this->userCamera->Load(NULL);
+ this->userCamera->Init();
++ this->userCamera->Translate( Vector3(5, 0, 3) );
++ this->userCamera->RotateYaw( DTOR(180) );
+ this->userCamera->RotatePitch( DTOR(30) );
+- this->userCamera->Translate( Vector3(-5, 0, 1) );
+
+ this->activeCamera = this->userCamera;
+ }
+@@ -235,6 +240,21 @@
this->activeCamera->RotateYaw(DTOR(-d.x * this->rotateAmount));
this->activeCamera->RotatePitch(DTOR(d.y * this->rotateAmount));
}
@@ -1033,7 +1064,7 @@
}
this->mouseDrag = true;
-@@ -247,12 +266,28 @@
+@@ -247,12 +267,28 @@
std::map<int,int>::iterator iter;
this->keys[keyNum] = 1;
@@ -1064,7 +1095,7 @@
this->moveAmount *= 2;
Index: gazebo-svn/server/World.hh
===================================================================
---- gazebo-svn/server/World.hh (revision 6715)
+--- gazebo-svn/server/World.hh (revision 6696)
+++ gazebo-svn/server/World.hh (working copy)
@@ -91,6 +91,26 @@
/// \return Pointer to the physics engine
@@ -1105,7 +1136,7 @@
};
Index: gazebo-svn/server/controllers/camera/stereo/Stereo_Camera.cc
===================================================================
---- gazebo-svn/server/controllers/camera/stereo/Stereo_Camera.cc (revision 6715)
+--- gazebo-svn/server/controllers/camera/stereo/Stereo_Camera.cc (revision 6696)
+++ gazebo-svn/server/controllers/camera/stereo/Stereo_Camera.cc (working copy)
@@ -87,26 +87,59 @@
// Update the controller
@@ -1182,7 +1213,7 @@
////////////////////////////////////////////////////////////////////////////////
Index: gazebo-svn/server/controllers/camera/generic/Generic_Camera.cc
===================================================================
---- gazebo-svn/server/controllers/camera/generic/Generic_Camera.cc (revision 6715)
+--- gazebo-svn/server/controllers/camera/generic/Generic_Camera.cc (revision 6696)
+++ gazebo-svn/server/controllers/camera/generic/Generic_Camera.cc (working copy)
@@ -79,7 +79,24 @@
// Update the controller
@@ -1212,7 +1243,7 @@
////////////////////////////////////////////////////////////////////////////////
Index: gazebo-svn/server/controllers/ControllerFactory.cc
===================================================================
---- gazebo-svn/server/controllers/ControllerFactory.cc (revision 6715)
+--- gazebo-svn/server/controllers/ControllerFactory.cc (revision 6696)
+++ gazebo-svn/server/controllers/ControllerFactory.cc (working copy)
@@ -42,6 +42,7 @@
// Register a controller class. Use by dynamically loaded modules
@@ -1224,7 +1255,7 @@
Index: gazebo-svn/server/controllers/laser/sicklms200/SickLMS200_Laser.cc
===================================================================
---- gazebo-svn/server/controllers/laser/sicklms200/SickLMS200_Laser.cc (revision 6715)
+--- gazebo-svn/server/controllers/laser/sicklms200/SickLMS200_Laser.cc (revision 6696)
+++ gazebo-svn/server/controllers/laser/sicklms200/SickLMS200_Laser.cc (working copy)
@@ -98,6 +98,7 @@
if (this->laserIface->Lock(1))
@@ -1245,7 +1276,7 @@
////////////////////////////////////////////////////////////////////////////////
Index: gazebo-svn/server/controllers/Controller.cc
===================================================================
---- gazebo-svn/server/controllers/Controller.cc (revision 6715)
+--- gazebo-svn/server/controllers/Controller.cc (revision 6696)
+++ gazebo-svn/server/controllers/Controller.cc (working copy)
@@ -141,10 +141,10 @@
/// Update the controller. Called every cycle.
@@ -1262,7 +1293,7 @@
Index: gazebo-svn/server/World.cc
===================================================================
---- gazebo-svn/server/World.cc (revision 6715)
+--- gazebo-svn/server/World.cc (revision 6696)
+++ gazebo-svn/server/World.cc (working copy)
@@ -27,6 +27,7 @@
#include <assert.h>
@@ -1354,7 +1385,7 @@
int World::LoadEntities(XMLConfigNode *node, Model *parent)
Index: gazebo-svn/SConstruct
===================================================================
---- gazebo-svn/SConstruct (revision 6715)
+--- gazebo-svn/SConstruct (revision 6696)
+++ gazebo-svn/SConstruct (working copy)
@@ -24,6 +24,8 @@
parseConfigs=['pkg-config --cflags --libs OGRE',
@@ -1367,7 +1398,7 @@
]
Index: gazebo-svn/worlds/stereocamera.world
===================================================================
---- gazebo-svn/worlds/stereocamera.world (revision 6715)
+--- gazebo-svn/worlds/stereocamera.world (revision 6696)
+++ gazebo-svn/worlds/stereocamera.world (working copy)
@@ -105,10 +105,10 @@
<saveFramePath>frames</saveFramePath>
Modified: pkg/trunk/demos/2dnav-gazebo/world/pr2.model
===================================================================
--- pkg/trunk/demos/2dnav-gazebo/world/pr2.model 2008-07-02 02:17:07 UTC (rev 1187)
+++ pkg/trunk/demos/2dnav-gazebo/world/pr2.model 2008-07-02 02:43:58 UTC (rev 1188)
@@ -503,8 +503,6 @@
<body2>dummytorso_body</body2>
<anchor>dummytorso_body</anchor>
<axis> 1.0 0.0 0.0 </axis>
- <cfm>0.001</cfm>
- <erp>0.6</erp>
</joint:hinge>
<joint:hinge name="tmp2dummy">
@@ -512,8 +510,6 @@
<body2>dummytorso_body</body2>
<anchor>dummytorso_body</anchor>
<axis> 0.0 1.0 0.0 </axis>
- <cfm>0.001</cfm>
- <erp>0.6</erp>
</joint:hinge>
<joint:hinge name="tmp3dummy">
@@ -521,8 +517,6 @@
<body2>dummytorso_body</body2>
<anchor>dummytorso_body</anchor>
<axis> 0.0 0.0 1.0 </axis>
- <cfm>0.001</cfm>
- <erp>0.6</erp>
</joint:hinge>
@@ -580,6 +574,8 @@
<xyz> 0.185 -0.15 0.68 </xyz>
<rpy> 0.0 0.0 0.0 </rpy>
<geom:cylinder name="shoulder_pan_body_right_geom">
+ <kp>1000000.0</kp>
+ <kd>1.0</kd>
<size> 0.13 0.6</size> <!-- radius and length -->
<mass>1.0</mass>
<visual>
@@ -597,6 +593,8 @@
<xyz> 0.285 -0.15 0.86 </xyz>
<rpy> 90.0 0.0 0.0 </rpy>
<geom:cylinder name="upperarm_joint_right_geom">
+ <kp>1000000.0</kp>
+ <kd>1.0</kd>
<size> 0.10 0.05</size> <!-- radius and length -->
<mass>1.0</mass>
<visual>
@@ -613,6 +611,8 @@
<xyz> 0.485 -0.15 0.86 </xyz>
<rpy> 0.0 0.0 0.0 </rpy>
<geom:box name="upperarm_right_geom">
+ <kp>1000000.0</kp>
+ <kd>1.0</kd>
<size> 0.25 0.1 0.1</size>
<mass>1.0</mass>
<visual>
@@ -629,6 +629,8 @@
<xyz> 0.6850 -0.15 0.86 </xyz>
<rpy> 90.0 0.0 0.0 </rpy>
<geom:cylinder name="elbow_right_geom">
+ <kp>1000000.0</kp>
+ <kd>1.0</kd>
<size> 0.04 0.1</size> <!-- radius and length -->
<mass>1.0</mass>
<visual>
@@ -645,6 +647,8 @@
<xyz> 0.84225 -0.15 0.86 </xyz>
<rpy> 0.0 0.0 0.0 </rpy>
<geom:box name="forearm_right_geom">
+ <kp>1000000.0</kp>
+ <kd>1.0</kd>
<size> 0.2 0.1 0.1</size>
<mass>1.0</mass>
<visual>
@@ -661,6 +665,8 @@
<xyz> 0.9995 -0.15 0.86 </xyz>
<rpy> 90.0 0.0 0.0 </rpy>
<geom:cylinder name="wrist_right_geom">
+ <kp>1000000.0</kp>
+ <kd>1.0</kd>
<size> 0.025 0.1</size> <!-- radius and length -->
<mass>1.0</mass>
<visual>
@@ -677,6 +683,8 @@
<xyz> 1.05 -0.15 0.86</xyz>
<rpy> 0.0 0.0 0.0 </rpy>
<geom:box name="palm_right_geom">
+ <kp>1000000.0</kp>
+ <kd>1.0</kd>
<size> 0.05 0.05 0.05</size>
<mass>1.0</mass>
<visual>
@@ -693,6 +701,8 @@
<xyz> 1.15 -0.17 0.86 </xyz>
<rpy> 0.0 0.0 0.0 </rpy>
<geom:box name="finger_1_right_geom">
+ <kp>1000000.0</kp>
+ <kd>1.0</kd>
<size> 0.05 0.01 0.025</size>
<mass>1.0</mass>
<visual>
@@ -707,6 +717,8 @@
<xyz> 1.15 -0.13 0.86 </xyz>
<rpy> 0.0 0.0 0.0 </rpy>
<geom:box name="finger_2_right_geom">
+ <kp>1000000.0</kp>
+ <kd>1.0</kd>
<size> 0.05 0.01 0.025</size>
<mass>1.0</mass>
<visual>
@@ -727,8 +739,6 @@
<body2>torso_body</body2>
<anchor>shoulder_pan_body_right</anchor>
<axis> 0.0 0.0 1.0 </axis>
- <cfm>0.001</cfm>
- <erp>0.6</erp>
</joint:hinge>
<joint:hinge name="upperarm_pitch_joint_right">
@@ -736,8 +746,6 @@
<body1>upperarm_joint_right</body1>
<anchor>upperarm_joint_right</anchor>
<axis> 0.0 1.0 0.0 </axis>
- <cfm>0.001</cfm>
- <erp>0.6</erp>
</joint:hinge>
<joint:hinge name="upperarm_roll_joint_right">
@@ -745,8 +753,6 @@
<body2>upperarm_joint_right</body2>
<anchor>upperarm_joint_right</anchor>
<axis> 1.0 0.0 0.0 </axis>
- <cfm>0.001</cfm>
- <erp>0.6</erp>
</joint:hinge>
<joint:hinge name="elbow_pitch_joint_right">
@@ -754,8 +760,6 @@
<body2>upperarm_right</body2>
<anchor>elbow_right</anchor>
<axis> 0.0 1.0 0.0 </axis>
- <cfm>0.001</cfm>
- <erp>0.6</erp>
</joint:hinge>
<joint:hinge name="forearm_roll_joint_right">
@@ -763,8 +767,6 @@
<body2>elbow_right</body2>
<anchor>elbow_right</anchor>
<axis> 1.0 0.0 0.0 </axis>
- <cfm>0.001</cfm>
- <erp>0.6</erp>
</joint:hinge>
<joint:hinge name="wrist_pitch_joint_right">
@@ -772,8 +774,6 @@
<body2>forearm_right</body2>
<anchor>wrist_right</anchor>
<axis> 0.0 1.0 0.0 </axis>
- <cfm>0.001</cfm>
- <erp>0.6</erp>
</joint:hinge>
<joint:hinge name="palm_roll_joint_right">
@@ -781,8 +781,6 @@
<body2>wrist_right</body2>
<anchor>wrist_right</anchor>
<axis> 1.0 0.0 0.0 </axis>
- <cfm>0.001</cfm>
- <erp>0.6</erp>
</joint:hinge>
<joint:slider name="finger_1_slider_right">
@@ -809,6 +807,8 @@
<xyz> 0.185 0.15 0.68 </xyz>
<rpy> 0.0 0.0 0.0 </rpy>
<geom:cylinder name="shoulder_pan_body_left_geom">
+ <kp>1000000.0</kp>
+ <kd>1.0</kd>
<size> 0.13 0.6</size> <!-- radius and length -->
<mass>1.0</mass>
<visual>
@@ -826,6 +826,8 @@
<xyz> 0.285 0.15 0.86 </xyz>
<rpy> 90.0 0.0 0.0 </rpy>
<geom:cylinder name="upperarm_joint_left_geom">
+ <kp>1000000.0</kp>
+ <kd>1.0</kd>
<size> 0.10 0.05</size> <!-- radius and length -->
<mass>1.0</mass>
<visual>
@@ -842,6 +844,8 @@
<xyz> 0.485 0.15 0.86 </xyz>
<rpy> 0.0 0.0 0.0 </rpy>
<geom:box name="upperarm_left_geom">
+ <kp>1000000.0</kp>
+ <kd>1.0</kd>
<size> 0.25 0.1 0.1</size>
<mass>1.0</mass>
<visual>
@@ -858,6 +862,8 @@
<xyz> 0.685 0.15 0.86 </xyz>
<rpy> 90.0 0.0 0.0 </rpy>
<geom:cylinder name="elbow_left_geom">
+ <kp>1000000.0</kp>
+ <kd>1.0</kd>
<size> 0.04 0.1</size> <!-- radius and length -->
<mass>1.0</mass>
<visual>
@@ -874,6 +880,8 @@
<xyz> 0.84225 0.15 0.86 </xyz>
<rpy> 0.0 0.0 0.0 </rpy>
<geom:box name="forearm_left_geom">
+ <kp>1000000.0</kp>
+ <kd>1.0</kd>
<size> 0.2 0.1 0.1</size>
<mass>1.0</mass>
<visual>
@@ -890,6 +898,8 @@
<xyz> 0.9995 0.15 0.86 </xyz>
<rpy> 90.0 0.0 0.0 </rpy>
<geom:cylinder name="wrist_left_geom">
+ <kp>1000000.0</kp>
+ <kd>1.0</kd>
<size> 0.025 0.1</size> <!-- radius and length -->
<mass>1.0</mass>
<visual>
@@ -906,6 +916,8 @@
<xyz> 1.05 0.15 0.86 </xyz>
<rpy> 0.0 0.0 0.0 </rpy>
<geom:box name="palm_left_geom">
+ <kp>1000000.0</kp>
+ <kd>1.0</kd>
<size> 0.05 0.05 0.05</size>
<mass>1.0</mass>
<visual>
@@ -922,6 +934,8 @@
<xyz> 1.15 0.13 0.86 </xyz>
<rpy> 0.0 0.0 0.0 </rpy>
<geom:box name="finger_1_left_geom">
+ <kp>1000000.0</kp>
+ <kd>1.0</kd>
<size> 0.05 0.01 0.025</size>
<mass>1.0</mass>
<visual>
@@ -936,6 +950,8 @@
<xyz> 1.15 0.17 0.86 </xyz>
<rpy> 0.0 0.0 0.0 </rpy>
<geom:box name="finger_2_left_geom">
+ <kp>1000000.0</kp>
+ <kd>1.0</kd>
<size> 0.05 0.01 0.025</size>
<mass>1.0</mass>
<visual>
@@ -952,8 +968,6 @@
<body2>torso_body</body2>
<anchor>shoulder_pan_body_left</anchor>
<axis> 0.0 0.0 1.0 </axis>
- <cfm>0.001</cfm>
- <erp>0.6</erp>
</joint:hinge>
<joint:hinge name="upperarm_pitch_joint_left">
@@ -961,8 +975,6 @@
<body2>shoulder_pan_body_left</body2>
<anchor>upperarm_joint_left</anchor>
<axis> 0.0 1.0 0.0 </axis>
- <cfm>0.001</cfm>
- <erp>0.6</erp>
</joint:hinge>
<joint:hinge name="upperarm_roll_joint_left">
@@ -970,8 +982,6 @@
<body2>upperarm_joint_left</body2>
<anchor>upperarm_joint_left</anchor>
<axis> 1.0 0.0 0.0 </axis>
- <cfm>0.001</cfm>
- <erp>0.6</erp>
</joint:hinge>
<joint:hinge name="elbow_pitch_joint_left">
@@ -979,8 +989,6 @@
<body2>upperarm_left</body2>
<anchor>elbow_left</anchor>
<axis> 0.0 1.0 0.0 </axis>
- <cfm>0.001</cfm>
- <erp>0.6</erp>
</joint:hinge>
<joint:hinge name="forearm_roll_joint_left">
@@ -988,8 +996,6 @@
<body2>elbow_left</body2>
<anchor>elbow_left</anchor>
<axis> 1.0 0.0 0.0 </axis>
- <cfm>0.001</cfm>
- <erp>0.6</erp>
</joint:hinge>
<joint:hinge name="wrist_pitch_joint_left">
@@ -997,8 +1003,6 @@
<body2>forearm_left</body2>
<anchor>wrist_left</anchor>
<axis> 0.0 1.0 0.0 </axis>
- <cfm>0.001</cfm>
- <erp>0.6</erp>
</joint:hinge>
<joint:hinge name="palm_roll_joint_left">
@@ -1006,8 +1010,6 @@
<body2>wrist_left</body2>
<anchor>wrist_left</anchor>
<axis> 1.0 0.0 0.0 </axis>
- <cfm>0.001</cfm>
- <erp>0.6</erp>
</joint:hinge>
<joint:slider name="finger_1_slider_left">
@@ -1046,6 +1048,10 @@
<reportStaticCollision>true</reportStaticCollision>
<geom:box name="front_left_caster_geom">
+ <kp>10000000.0</kp>
+ <kd>1.0</kd>
+ <mu1>10.0</mu1>
+ <mu2>10.0</mu2>
<xyz> 0.0 0.0 0.0</xyz>
<size>0.1 0.08 0.1</size> <!-- dx, dy, dz -->
<mass>1.0</mass>
@@ -1065,6 +1071,10 @@
<reportStaticCollision>true</reportStaticCollision>
<geom:box name="front_right_caster_geom">
+ <kp>10000000.0</kp>
+ <kd>1.0</kd>
+ <mu1>10.0</mu1>
+ <mu2>10.0</mu2>
<xyz>0 0 0.0</xyz>
<size>0.1 0.08 0.1</size> <!-- dx, dy, dz -->
<mass>1.0</mass>
@@ -1084,6 +1094,10 @@
<reportStaticCollision>true</reportStaticCollision>
<geom:box name="rear_left_caster_geom">
+ <kp>10000000.0</kp>
+ <kd>1.0</kd>
+ <mu1>10.0</mu1>
+ <mu2>10.0</mu2>
<xyz>0 0 0.0</xyz>
<size>0.1 0.08 0.1</size> <!-- dx, dy, dz -->
<mass>1.0</mass>
@@ -1102,6 +1116,10 @@
<reportStaticCollision>true</reportStaticCollision>
<geom:box name="rear_right_caster_geom">
+ <kp>10000000.0</kp>
+ <kd>1.0</kd>
+ <mu1>10.0</mu1>
+ <mu2>10.0</mu2>
<xyz>0 0 0.0</xyz>
<size>0.1 0.08 0.1</size> <!-- dx, dy, dz -->
<mass>1.0</mass>
@@ -1119,6 +1137,10 @@
<xyz> 0.425 0.305 -0.17</xyz>
<rpy> 90.0 0.0 1.0 </rpy>
<geom:cylinder name="front_left_wheel_l_geom">
+ <kp>10000000.0</kp>
+ <kd>1.0</kd>
+ <mu1>10.0</mu1>
+ <mu2>10.0</mu2>
<size> 0.08 0.03</size> <!-- radius and width -->
<mass>0.5</mass>
<visual>
@@ -1133,6 +1155,10 @@
<xyz> 0.425 0.195 -0.17</xyz>
<rpy> 90.0 0.0 1.0 </rpy>
<geom:cylinder name="front_left_wheel_r_geom">
+ <kp>10000000.0</kp>
+ <kd>1.0</kd>
+ <mu1>10.0</mu1>
+ <mu2>10.0</mu2>
<size> 0.08 0.03</size> <!-- radius and width -->
<mass>0.5</mass>
<visual>
@@ -1147,6 +1173,10 @@
<xyz> 0.425 -0.195 -0.17</xyz>
<rpy> 90.0 0.0 0.0 </rpy>
<geom:cylinder name="front_right_wheel_l_geom">
+ <kp>10000000.0</kp>
+ <kd>1.0</kd>
+ <mu1>10.0</mu1>
+ <mu2>10.0</mu2>
<size> 0.08 0.03</size> <!-- radius and width -->
<mass>0.5</mass>
<visual>
@@ -1161,6 +1191,10 @@
<xyz> 0.425 -0.305 -0.17</xyz>
<rpy> 90.0 0.0 0.0 </rpy>
<geom:cylinder name="front_right_wheel_r_geom">
+ <kp>10000000.0</kp>
+ <kd>1.0</kd>
+ <mu1>10.0</mu1>
+ <mu2>10.0</mu2>
<size> 0.08 0.03</size> <!-- radius and width -->
<mass>0.5</mass>
<visual>
@@ -1175,6 +1209,10 @@
<xyz> -0.075 0.305 -0.17</xyz>
<rpy> 90.0 0.0 0.0 </rpy>
<geom:cylinder name="rear_left_wheel_l_geom">
+ <kp>10000000.0</kp>
+ <kd>1.0</kd>
+ <mu1>10.0</mu1>
+ <mu2>10.0</mu2>
<size> 0.08 0.03</size> <!-- radius and width -->
<mass>0.5</mass>
<visual>
@@ -1189,6 +1227,10 @@
<xyz> -0.075 0.195 -0.17</xyz>
<rpy> 90.0 0.0 0.0 </rpy>
<geom:cylinder name="rear_left_wheel_r_geom">
+ <kp>10000000.0</kp>
+ <kd>1.0</kd>
+ <mu1>10.0</mu1>
+ <mu2>10.0</mu2>
<size> 0.08 0.03</size> <!-- radius and width -->
<mass>0.5</mass>
<visual>
@@ -1203,6 +1245,10 @@
<xyz> -0.075 -0.195 -0.17</xyz>
<rpy> 90.0 0.0 0.0 </rpy>
<geom:cylinder name="rear_right_wheel_l_geom">
+ <kp>10000000.0</kp>
+ <kd>1.0</kd>
+ <mu1>10.0</mu1>
+ <mu2>10.0</mu2>
<size> 0.08 0.03</size> <!-- radius and width -->
<mass>0.5</mass>
<visual>
@@ -1217,6 +1263,10 @@
<xyz> -0.075 -0.305 -0.17</xyz>
<rpy> 90.0 0.0 0.0 </rpy>
<geom:cylinder name="rear_right_wheel_r_geom">
+ <kp>10000000.0</kp>
+ <kd>1.0</kd>
+ <mu1>10.0</mu1>
+ <mu2>10.0</mu2>
<size> 0.08 0.03</size> <!-- radius and width -->
<mass>0.5</mass>
<visual>
@@ -1235,8 +1285,6 @@
<body2>front_left_wheel_l</body2>
<anchor>front_left_wheel_l</anchor>
<axis>0 1 0</axis>
- <cfm>0.01</cfm>
- <erp>0.6</erp>
</joint:hinge>
<joint:hinge name="front_left_wheel_r_drive">
@@ -1244,8 +1292,6 @@
<body2>front_left_wheel_r</body2>
<anchor>front_left_wheel_r</anchor>
<axis>0 1 0</axis>
- <cfm>0.01</cfm>
- <erp>0.6</erp>
</joint:hinge>
<joint:hinge name="front_right_wheel_l_drive">
@@ -1253,8 +1299,6 @@
<body2>front_right_wheel_l</body2>
<anchor>front_right_wheel_l</anchor>
<axis>0 1 0</axis>
- <cfm>0.01</cfm>
- <erp>0.6</erp>
</joint:hinge>
<joint:hinge name="front_right_wheel_r_drive">
@@ -1262,8 +1306,6 @@
<body2>front_right_wheel_r</body2>
<anchor>front_right_wheel_r</anchor>
<axis>0 1 0</axis>
- <cfm>0.01</cfm>
- <erp>0.6</erp>
</joint:hinge>
<joint:hinge name="rear_left_wheel_l_drive">
@@ -1271,8 +1313,6 @@
<body2>rear_left_wheel_l</body2>
<anchor>rear_left_wheel_l</anchor>
<axis>0 1 0</axis>
- <cfm>0.01</cfm>
- <erp>0.6</erp>
</joint:hinge>
<joint:hinge name="rear_left_wheel_r_drive">
@@ -1280,8 +1320,6 @@
<body2>rear_left_wheel_r</body2>
<anchor>rear_left_wheel_r</anchor>
<axis>0 1 0</axis>
- <cfm>0.01</cfm>
- <erp>0.6</erp>
</joint:hinge>
<joint:hinge name="rear_right_wheel_l_drive">
@@ -1289,8 +1327,6 @@
<body2>rear_right_wheel_l</body2>
<anchor>rear_right_wheel_l</anchor>
<axis>0 1 0</axis>
- <cfm>0.01</cfm>
- <erp>0.6</erp>
</joint:hinge>
<joint:hinge name="rear_right_wheel_r_drive">
@@ -1298,8 +1334,6 @@
<body2>rear_right_wheel_r</body2>
<anchor>rear_right_wheel_r</anchor>
<axis>0 1 0</axis>
- <cfm>0.01</cfm>
- <erp>0.6</erp>
</joint:hinge>
@@ -1310,8 +1344,6 @@
<body2>base_body</body2>
<anchor>front_left_caster_body</anchor>
<axis> 0.0 0.0 1.0 </axis>
- <cfm>0.01</cfm>
- <erp>0.6</erp>
</joint:hinge>
<joint:hinge name="front_right_caster_steer">
@@ -1319,8 +1351,6 @@
<body2>base_body</body2>
<anchor>front_right_caster_body</anchor>
<axis> 0.0 0.0 1.0 </axis>
- <cfm>0.01</cfm>
- <erp>0.6</erp>
</joint:hinge>
<joint:hinge name="rear_left_caster_steer">
@@ -1328,8 +1358,6 @@
<body2>base_body</body2>
<anchor>rear_left_caster_body</anchor>
<axis> 0.0 0.0 1.0 </axis>
- <cfm>0.01</cfm>
- <erp>0.6</erp>
</joint:hinge>
<joint:hinge name="rear_right_caster_steer">
@@ -1337,8 +1365,6 @@
<body2>base_body</body2>
<anchor>rear_right_caster_body</anchor>
<axis> 0.0 0.0 1.0 </axis>
- <cfm>0.01</cfm>
- <erp>0.6</erp>
</joint:hinge>
Modified: pkg/trunk/demos/2dnav-gazebo/world/robot.world
===================================================================
--- pkg/trunk/demos/2dnav-gazebo/world/robot.world 2008-07-02 02:17:07 UTC (rev 1187)
+++ pkg/trunk/demos/2dnav-gazebo/world/robot.world 2008-07-02 02:43:58 UTC (rev 1188)
@@ -22,11 +22,12 @@
<!-- cfm is 1e-5 for single precision -->
<!-- erp is typically .1-.8 -->
+<!-- here's the global contact cfm/erp -->
<physics:ode>
<stepTime>0.01</stepTime>
<gravity>0 0 -9.8</gravity>
- <erp>0.99</erp>
- <cfm>0.001</cfm>
+ <cfm>0.00001</cfm>
+ <erp>1.0</erp>
</physics:ode>
<geo:origin>
@@ -69,6 +70,8 @@
<body:plane name="plane">
<geom:plane name="plane">
+ <kp>1000000.0</kp>
+ <kd>1.0</kd>
<normal>0 0 1</normal>
<size>51.3 51.3</size>
<!-- map3.png -->
@@ -157,12 +160,12 @@
<rpy> 0.0 0.0 0.00</rpy>
<body:box name="desk1_legs_body">
<geom:box name="desk1_legs_geom">
+ <kp>1000000.0</kp>
+ <kd>1.0</kd>
<xyz> 0.0 0.0 0.50</xyz>
<mesh>default</mesh>
<size>0.5 1.0 0.75</size>
<mass> 10.0</mass>
- <cfm>0.000001</cfm>
- <erp>0.8</erp>
<visual>
<size> 0.5 1.0 0.75</size>
<material>Gazebo/Rocky</material>
@@ -170,12 +173,12 @@
</visual>
</geom:box>
<geom:box name="desk1_top_geom">
+ <kp>1000000.0</kp>
+ <kd>1.0</kd>
<xyz> 0.0 0.0 0.90</xyz>
<mesh>default</mesh>
<size>0.75 1.5 0.05</size>
<mass> 10.0</mass>
- <cfm>0.000001</cfm>
- <erp>0.8</erp>
<visual>
<size> 0.75 1.5 0.05</size>
<material>Gazebo/Rocky</material>
@@ -191,12 +194,12 @@
<rpy> 0.0 0.0 0.00</rpy>
<body:box name="desk2_legs_body">
<geom:box name="desk2_legs_geom">
+ <kp>1000000.0</kp>
+ <kd>1.0</kd>
<xyz> 0.0 0.0 0.50</xyz>
<mesh>default</mesh>
<size>0.5 1.0 0.75</size>
<mass> 10.0</mass>
- <cfm>0.000001</cfm>
- <erp>0.8</erp>
<visual>
<size> 0.5 1.0 0.75</size>
<material>Gazebo/Rocky</material>
@@ -204,12 +207,12 @@
</visual>
</geom:box>
<geom:box name="desk2_top_geom">
+ <kp>1000000.0</kp>
+ <kd>1.0</kd>
<xyz> 0.0 0.0 0.90</xyz>
<mesh>default</mesh>
<size>0.75 1.5 0.05</size>
<mass> 10.0</mass>
- <cfm>0.001</cfm>
- <erp>0.8</erp>
<visual>
<size> 0.75 1.5 0.05</size>
<material>Gazebo/Rocky</material>
@@ -223,10 +226,10 @@
<model:physical name="cylinder1_model">
<xyz> 2.5 0.0 0.9</xyz>
<rpy> 0.0 0.0 0.0...
[truncated message content] |
|
From: <hsu...@us...> - 2008-07-02 18:38:04
|
Revision: 1208
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=1208&view=rev
Author: hsujohnhsu
Date: 2008-07-02 11:37:07 -0700 (Wed, 02 Jul 2008)
Log Message:
-----------
fixed indentation and moved break out of if's.
Modified Paths:
--------------
pkg/trunk/drivers/robot/pr2/libpr2API/src/pr2API.cpp
pkg/trunk/simulators/gazebo_plugin/src/Pr2_Actarray.cc
Modified: pkg/trunk/drivers/robot/pr2/libpr2API/src/pr2API.cpp
===================================================================
--- pkg/trunk/drivers/robot/pr2/libpr2API/src/pr2API.cpp 2008-07-02 18:33:32 UTC (rev 1207)
+++ pkg/trunk/drivers/robot/pr2/libpr2API/src/pr2API.cpp 2008-07-02 18:37:07 UTC (rev 1208)
@@ -152,29 +152,29 @@
PR2_ERROR_CODE PR2Robot::SetArmCartesianPosition(PR2_MODEL_ID id, const KDL::Frame &f, const KDL::JntArray &q_init, KDL::JntArray &q_out)
{
- // KDL::JntArray q_init = KDL::JntArray(this->pr2_kin.nJnts);
- // q_init(0) = 0.1, q_init(1) = 0.0, q_init(2) = 0.0, q_init(3) = 0.0;
- // q_init(4) = 0.0, q_init(5) = 0.0, q_init(6) = 0.0;
+ // KDL::JntArray q_init = KDL::JntArray(this->pr2_kin.nJnts);
+ // q_init(0) = 0.1, q_init(1) = 0.0, q_init(2) = 0.0, q_init(3) = 0.0;
+ // q_init(4) = 0.0, q_init(5) = 0.0, q_init(6) = 0.0;
- // KDL::JntArray q_out = KDL::JntArray(this->pr2_kin.nJnts);
- if (this->pr2_kin.IK(q_init, f, q_out) == true)
- cout<<"IK result:"<<q_out<<endl;
- else
- cout<<"Could not compute Inv Kin."<<endl;
+ // KDL::JntArray q_out = KDL::JntArray(this->pr2_kin.nJnts);
+ if (this->pr2_kin.IK(q_init, f, q_out) == true)
+ cout<<"IK result:"<<q_out<<endl;
+ else
+ cout<<"Could not compute Inv Kin."<<endl;
- //------ checking that IK returned a valid soln -----
- KDL::Frame f_ik;
- if (this->pr2_kin.FK(q_out,f_ik))
- {
- // cout<<"End effector after IK:"<<f_ik<<endl;
- }
- else
- cout<<"Could not compute Fwd Kin. (After IK)"<<endl;
+ //------ checking that IK returned a valid soln -----
+ KDL::Frame f_ik;
+ if (this->pr2_kin.FK(q_out,f_ik))
+ {
+ // cout<<"End effector after IK:"<<f_ik<<endl;
+ }
+ else
+ cout<<"Could not compute Fwd Kin. (After IK)"<<endl;
- for(int ii = 0; ii < 7; ii++)
- hw.SetJointServoCmd((PR2::PR2_JOINT_ID) (JointStart[id]+ii),q_out(ii),0);
+ for(int ii = 0; ii < 7; ii++)
+ hw.SetJointServoCmd((PR2::PR2_JOINT_ID) (JointStart[id]+ii),q_out(ii),0);
- return PR2_ALL_OK;
+ return PR2_ALL_OK;
}
PR2_ERROR_CODE PR2Robot::SetArmCartesianPosition(PR2_MODEL_ID id, NEWMAT::Matrix g)
@@ -437,99 +437,103 @@
{
switch(id)
{
-
+
case PR2_LEFT_ARM:
- if(mode == PR2_CARTESIAN_CONTROL){
- armControlMode[0] = mode;
- hw.SetJointControlMode(ARM_L_PAN , SPEED_CONTROL);
- hw.SetJointControlMode(ARM_L_SHOULDER_PITCH , SPEED_CONTROL);
- hw.SetJointControlMode(ARM_L_SHOULDER_ROLL , SPEED_CONTROL);
- hw.SetJointControlMode(ARM_L_ELBOW_PITCH , SPEED_CONTROL);
- hw.SetJointControlMode(ARM_L_ELBOW_ROLL , SPEED_CONTROL);
- hw.SetJointControlMode(ARM_L_WRIST_PITCH , SPEED_CONTROL);
- hw.SetJointControlMode(ARM_L_WRIST_ROLL , SPEED_CONTROL);
- break;
- }
- else if(mode == PR2_SPEED_TORQUE_CONTROL){
- armControlMode[0] = mode;
- hw.SetJointControlMode(ARM_L_PAN , SPEED_TORQUE_CONTROL);
- hw.SetJointControlMode(ARM_L_SHOULDER_PITCH , SPEED_TORQUE_CONTROL);
- hw.SetJointControlMode(ARM_L_SHOULDER_ROLL , SPEED_TORQUE_CONTROL);
- hw.SetJointControlMode(ARM_L_ELBOW_PITCH , SPEED_TORQUE_CONTROL);
- hw.SetJointControlMode(ARM_L_ELBOW_ROLL , SPEED_TORQUE_CONTROL);
- hw.SetJointControlMode(ARM_L_WRIST_PITCH , SPEED_TORQUE_CONTROL);
- hw.SetJointControlMode(ARM_L_WRIST_ROLL , SPEED_TORQUE_CONTROL);
- break;
- }
- else if(mode==PR2_CARTESIAN_TORQUE_CONTROL){
- armControlMode[0] = mode;
- hw.SetJointControlMode(ARM_L_PAN , PD_TORQUE_CONTROL);
- hw.SetJointControlMode(ARM_L_SHOULDER_PITCH , PD_TORQUE_CONTROL);
- hw.SetJointControlMode(ARM_L_SHOULDER_ROLL , PD_TORQUE_CONTROL);
- hw.SetJointControlMode(ARM_L_ELBOW_PITCH , PD_TORQUE_CONTROL);
- hw.SetJointControlMode(ARM_L_ELBOW_ROLL , PD_TORQUE_CONTROL);
- hw.SetJointControlMode(ARM_L_WRIST_PITCH , PD_TORQUE_CONTROL);
- hw.SetJointControlMode(ARM_L_WRIST_ROLL , PD_TORQUE_CONTROL);
- break;
- }
- else{ //Default to joint control mode
- armControlMode[0] = mode;
- hw.SetJointControlMode(ARM_L_PAN , PD_CONTROL);
- hw.SetJointControlMode(ARM_L_SHOULDER_PITCH , PD_CONTROL);
- hw.SetJointControlMode(ARM_L_SHOULDER_ROLL , PD_CONTROL);
- hw.SetJointControlMode(ARM_L_ELBOW_PITCH , PD_CONTROL);
- hw.SetJointControlMode(ARM_L_ELBOW_ROLL , PD_CONTROL);
- hw.SetJointControlMode(ARM_L_WRIST_PITCH , PD_CONTROL);
- hw.SetJointControlMode(ARM_L_WRIST_ROLL , PD_CONTROL);
- break;
- }
+ if(mode == PR2_CARTESIAN_CONTROL)
+ {
+ armControlMode[0] = mode;
+ hw.SetJointControlMode(ARM_L_PAN , SPEED_CONTROL);
+ hw.SetJointControlMode(ARM_L_SHOULDER_PITCH , SPEED_CONTROL);
+ hw.SetJointControlMode(ARM_L_SHOULDER_ROLL , SPEED_CONTROL);
+ hw.SetJointControlMode(ARM_L_ELBOW_PITCH , SPEED_CONTROL);
+ hw.SetJointControlMode(ARM_L_ELBOW_ROLL , SPEED_CONTROL);
+ hw.SetJointControlMode(ARM_L_WRIST_PITCH , SPEED_CONTROL);
+ hw.SetJointControlMode(ARM_L_WRIST_ROLL , SPEED_CONTROL);
+ }
+ else if(mode == PR2_SPEED_TORQUE_CONTROL)
+ {
+ armControlMode[0] = mode;
+ hw.SetJointControlMode(ARM_L_PAN , SPEED_TORQUE_CONTROL);
+ hw.SetJointControlMode(ARM_L_SHOULDER_PITCH , SPEED_TORQUE_CONTROL);
+ hw.SetJointControlMode(ARM_L_SHOULDER_ROLL , SPEED_TORQUE_CONTROL);
+ hw.SetJointControlMode(ARM_L_ELBOW_PITCH , SPEED_TORQUE_CONTROL);
+ hw.SetJointControlMode(ARM_L_ELBOW_ROLL , SPEED_TORQUE_CONTROL);
+ hw.SetJointControlMode(ARM_L_WRIST_PITCH , SPEED_TORQUE_CONTROL);
+ hw.SetJointControlMode(ARM_L_WRIST_ROLL , SPEED_TORQUE_CONTROL);
+ }
+ else if(mode==PR2_CARTESIAN_TORQUE_CONTROL)
+ {
+ armControlMode[0] = mode;
+ hw.SetJointControlMode(ARM_L_PAN , PD_TORQUE_CONTROL);
+ hw.SetJointControlMode(ARM_L_SHOULDER_PITCH , PD_TORQUE_CONTROL);
+ hw.SetJointControlMode(ARM_L_SHOULDER_ROLL , PD_TORQUE_CONTROL);
+ hw.SetJointControlMode(ARM_L_ELBOW_PITCH , PD_TORQUE_CONTROL);
+ hw.SetJointControlMode(ARM_L_ELBOW_ROLL , PD_TORQUE_CONTROL);
+ hw.SetJointControlMode(ARM_L_WRIST_PITCH , PD_TORQUE_CONTROL);
+ hw.SetJointControlMode(ARM_L_WRIST_ROLL , PD_TORQUE_CONTROL);
+ }
+ else
+ {
+ //Default to joint control mode
+ armControlMode[0] = mode;
+ hw.SetJointControlMode(ARM_L_PAN , PD_CONTROL);
+ hw.SetJointControlMode(ARM_L_SHOULDER_PITCH , PD_CONTROL);
+ hw.SetJointControlMode(ARM_L_SHOULDER_ROLL , PD_CONTROL);
+ hw.SetJointControlMode(ARM_L_ELBOW_PITCH , PD_CONTROL);
+ hw.SetJointControlMode(ARM_L_ELBOW_ROLL , PD_CONTROL);
+ hw.SetJointControlMode(ARM_L_WRIST_PITCH , PD_CONTROL);
+ hw.SetJointControlMode(ARM_L_WRIST_ROLL , PD_CONTROL);
+ }
+ break;
case PR2_RIGHT_ARM:
- if(mode == PR2_CARTESIAN_CONTROL){
- armControlMode[0] = mode;
- hw.SetJointControlMode(ARM_R_PAN , SPEED_CONTROL);
- hw.SetJointControlMode(ARM_R_SHOULDER_PITCH , SPEED_CONTROL);
- hw.SetJointControlMode(ARM_R_SHOULDER_ROLL , SPEED_CONTROL);
- hw.SetJointControlMode(ARM_R_ELBOW_PITCH , SPEED_CONTROL);
- hw.SetJointControlMode(ARM_R_ELBOW_ROLL , SPEED_CONTROL);
- hw.SetJointControlMode(ARM_R_WRIST_PITCH , SPEED_CONTROL);
- hw.SetJointControlMode(ARM_R_WRIST_ROLL , SPEED_CONTROL);
- break;
- }
- else if(mode == PR2_SPEED_TORQUE_CONTROL){
- armControlMode[0] = mode;
- hw.SetJointControlMode(ARM_R_PAN , SPEED_TORQUE_CONTROL);
- hw.SetJointControlMode(ARM_R_SHOULDER_PITCH , SPEED_TORQUE_CONTROL);
- hw.SetJointControlMode(ARM_R_SHOULDER_ROLL , SPEED_TORQUE_CONTROL);
- hw.SetJointControlMode(ARM_R_ELBOW_PITCH , SPEED_TORQUE_CONTROL);
- hw.SetJointControlMode(ARM_R_ELBOW_ROLL , SPEED_TORQUE_CONTROL);
- hw.SetJointControlMode(ARM_R_WRIST_PITCH , SPEED_TORQUE_CONTROL);
- hw.SetJointControlMode(ARM_R_WRIST_ROLL , SPEED_TORQUE_CONTROL);
- break;
- }
- else if(mode==PR2_CARTESIAN_TORQUE_CONTROL){
- armControlMode[0] = mode;
- hw.SetJointControlMode(ARM_R_PAN , PD_TORQUE_CONTROL);
- hw.SetJointControlMode(ARM_R_SHOULDER_PITCH , PD_TORQUE_CONTROL);
- hw.SetJointControlMode(ARM_R_SHOULDER_ROLL , PD_TORQUE_CONTROL);
- hw.SetJointControlMode(ARM_R_ELBOW_PITCH , PD_TORQUE_CONTROL);
- hw.SetJointControlMode(ARM_R_ELBOW_ROLL , PD_TORQUE_CONTROL);
- hw.SetJointControlMode(ARM_R_WRIST_PITCH , PD_TORQUE_CONTROL);
- hw.SetJointControlMode(ARM_R_WRIST_ROLL , PD_TORQUE_CONTROL);
- break;
- }
- else { //Default to joint control mode
- armControlMode[0] = mode;
- hw.SetJointControlMode(ARM_R_PAN , PD_CONTROL);
- hw.SetJointControlMode(ARM_R_SHOULDER_PITCH , PD_CONTROL);
- hw.SetJointControlMode(ARM_R_SHOULDER_ROLL , PD_CONTROL);
- hw.SetJointControlMode(ARM_R_ELBOW_PITCH , PD_CONTROL);
- hw.SetJointControlMode(ARM_R_ELBOW_ROLL , PD_CONTROL);
- hw.SetJointControlMode(ARM_R_WRIST_PITCH , PD_CONTROL);
- hw.SetJointControlMode(ARM_R_WRIST_ROLL , PD_CONTROL);
- break;
- }
- default:
- break;
+ if(mode == PR2_CARTESIAN_CONTROL)
+ {
+ armControlMode[0] = mode;
+ hw.SetJointControlMode(ARM_R_PAN , SPEED_CONTROL);
+ hw.SetJointControlMode(ARM_R_SHOULDER_PITCH , SPEED_CONTROL);
+ hw.SetJointControlMode(ARM_R_SHOULDER_ROLL , SPEED_CONTROL);
+ hw.SetJointControlMode(ARM_R_ELBOW_PITCH , SPEED_CONTROL);
+ hw.SetJointControlMode(ARM_R_ELBOW_ROLL , SPEED_CONTROL);
+ hw.SetJointControlMode(ARM_R_WRIST_PITCH , SPEED_CONTROL);
+ hw.SetJointControlMode(ARM_R_WRIST_ROLL , SPEED_CONTROL);
+ }
+ else if(mode == PR2_SPEED_TORQUE_CONTROL)
+ {
+ armControlMode[0] = mode;
+ hw.SetJointControlMode(ARM_R_PAN , SPEED_TORQUE_CONTROL);
+ hw.SetJointControlMode(ARM_R_SHOULDER_PITCH , SPEED_TORQUE_CONTROL);
+ hw.SetJointControlMode(ARM_R_SHOULDER_ROLL , SPEED_TORQUE_CONTROL);
+ hw.SetJointControlMode(ARM_R_ELBOW_PITCH , SPEED_TORQUE_CONTROL);
+ hw.SetJointControlMode(ARM_R_ELBOW_ROLL , SPEED_TORQUE_CONTROL);
+ hw.SetJointControlMode(ARM_R_WRIST_PITCH , SPEED_TORQUE_CONTROL);
+ hw.SetJointControlMode(ARM_R_WRIST_ROLL , SPEED_TORQUE_CONTROL);
+ }
+ else if(mode==PR2_CARTESIAN_TORQUE_CONTROL)
+ {
+ armControlMode[0] = mode;
+ hw.SetJointControlMode(ARM_R_PAN , PD_TORQUE_CONTROL);
+ hw.SetJointControlMode(ARM_R_SHOULDER_PITCH , PD_TORQUE_CONTROL);
+ hw.SetJointControlMode(ARM_R_SHOULDER_ROLL , PD_TORQUE_CONTROL);
+ hw.SetJointControlMode(ARM_R_ELBOW_PITCH , PD_TORQUE_CONTROL);
+ hw.SetJointControlMode(ARM_R_ELBOW_ROLL , PD_TORQUE_CONTROL);
+ hw.SetJointControlMode(ARM_R_WRIST_PITCH , PD_TORQUE_CONTROL);
+ hw.SetJointControlMode(ARM_R_WRIST_ROLL , PD_TORQUE_CONTROL);
+ }
+ else
+ {
+ //Default to joint control mode
+ armControlMode[0] = mode;
+ hw.SetJointControlMode(ARM_R_PAN , PD_CONTROL);
+ hw.SetJointControlMode(ARM_R_SHOULDER_PITCH , PD_CONTROL);
+ hw.SetJointControlMode(ARM_R_SHOULDER_ROLL , PD_CONTROL);
+ hw.SetJointControlMode(ARM_R_ELBOW_PITCH , PD_CONTROL);
+ hw.SetJointControlMode(ARM_R_ELBOW_ROLL , PD_CONTROL);
+ hw.SetJointControlMode(ARM_R_WRIST_PITCH , PD_CONTROL);
+ hw.SetJointControlMode(ARM_R_WRIST_ROLL , PD_CONTROL);
+ }
+ break;
+ default:
+ break;
}
return PR2_ALL_OK;
};
@@ -579,16 +583,16 @@
PR2_ERROR_CODE PR2Robot::GetArmJointPositionCmd(PR2_MODEL_ID id, KDL::JntArray &q)
{
- if (id != PR2_RIGHT_ARM && id != PR2_LEFT_ARM)
- return PR2_ERROR;
+ if (id != PR2_RIGHT_ARM && id != PR2_LEFT_ARM)
+ return PR2_ERROR;
- double pos,vel;
- for(int ii = JointStart[id]; ii <= JointEnd[id]; ii++)
- {
- hw.GetJointServoCmd((PR2_JOINT_ID)ii,&pos,&vel);
- q(ii-JointStart[id]) = pos;
- }
- return PR2_ALL_OK;
+ double pos,vel;
+ for(int ii = JointStart[id]; ii <= JointEnd[id]; ii++)
+ {
+ hw.GetJointServoCmd((PR2_JOINT_ID)ii,&pos,&vel);
+ q(ii-JointStart[id]) = pos;
+ }
+ return PR2_ALL_OK;
};
@@ -912,3 +916,4 @@
return hw.GetSimTime(time);
}
+
Modified: pkg/trunk/simulators/gazebo_plugin/src/Pr2_Actarray.cc
===================================================================
--- pkg/trunk/simulators/gazebo_plugin/src/Pr2_Actarray.cc 2008-07-02 18:33:32 UTC (rev 1207)
+++ pkg/trunk/simulators/gazebo_plugin/src/Pr2_Actarray.cc 2008-07-02 18:37:07 UTC (rev 1208)
@@ -200,8 +200,10 @@
);
this->pids[count]->SetCurrentCmd(0);
// as a first hack, initialize to zero velocity and saturation torque
- this->joints[count]->SetParam( dParamVel , this->pids[count]->GetCurrentCmd());
- this->joints[count]->SetParam( dParamFMax, this->myIface->data->actuators[count].saturationTorque );
+ //this->joints[count]->SetParam( dParamVel , this->pids[count]->GetCurrentCmd());
+ //this->joints[count]->SetParam( dParamFMax, this->myIface->data->actuators[count].saturationTorque );
+ this->joints[count]->SetParam( dParamVel , 0);
+ this->joints[count]->SetParam( dParamFMax, 0);
}
lastTime = Simulator::Instance()->GetSimTime();
}
@@ -276,35 +278,35 @@
switch(this->myIface->data->actuators[count].controlMode)
{
case PR2::TORQUE_CONTROL :
- sjoint->SetSliderForce(this->myIface->data->actuators[count].cmdEffectorForce);
- break;
- // case PR2::PD_CONTROL1 :
- // // No fancy controller, just pass the commanded torque/force in (we are not modeling the motors for now)
- // positionError = cmdPosition - sjoint->GetPosition();
- // speedError = cmdSpeed - sjoint->GetPositionRate();
- // //std::cout << "slider e:" << speedError << " + " << positionError << std::endl;
- // currentCmd = this->pids[count]->UpdatePid(positionError + 0.0*speedError, currentTime-this->lastTime);
- // currentCmd = (currentCmd > this->myIface->data->actuators[count].saturationTorque ) ? this->myIface->data->actuators[count].saturationTorque : currentCmd;
- // currentCmd = (currentCmd < -this->myIface->data->actuators[count].saturationTorque ) ? -this->myIface->data->actuators[count].saturationTorque : currentCmd;
- // sjoint->SetSliderForce(currentCmd);
- // break;
+ sjoint->SetSliderForce(this->myIface->data->actuators[count].cmdEffectorForce);
+ break;
+ // case PR2::PD_CONTROL1 :
+ // // No fancy controller, just pass the commanded torque/force in (we are not modeling the motors for now)
+ // positionError = cmdPosition - sjoint->GetPosition();
+ // speedError = cmdSpeed - sjoint->GetPositionRate();
+ // //std::cout << "slider e:" << speedError << " + " << positionError << std::endl;
+ // currentCmd = this->pids[count]->UpdatePid(positionError + 0.0*speedError, currentTime-this->lastTime);
+ // currentCmd = (currentCmd > this->myIface->data->actuators[count].saturationTorque ) ? this->myIface->data->actuators[count].saturationTorque : currentCmd;
+ // currentCmd = (currentCmd < -this->myIface->data->actuators[count].saturationTorque ) ? -this->myIface->data->actuators[count].saturationTorque : currentCmd;
+ // sjoint->SetSliderForce(currentCmd);
+ // break;
case PR2::PD_CONTROL : // velocity control
- //if (cmdPosition > sjoint->GetHighStop())
- // cmdPosition = sjoint->GetHighStop();
- //else if (cmdPosition < sjoint->GetLowStop())
- // cmdPosition = sjoint->GetLowStop();
+ //if (cmdPosition > sjoint->GetHighStop())
+ // cmdPosition = sjoint->GetHighStop();
+ //else if (cmdPosition < sjoint->GetLowStop())
+ // cmdPosition = sjoint->GetLowStop();
- positionError = cmdPosition - sjoint->GetPosition();
- speedError = cmdSpeed - sjoint->GetPositionRate();
- //std::cout << "slider e:" << speedError << " + " << positionError << std::endl;
- currentCmd = this->pids[count]->UpdatePid(positionError + 0.0*speedError, currentTime-this->lastTime);
+ positionError = cmdPosition - sjoint->GetPosition();
+ speedError = cmdSpeed - sjoint->GetPositionRate();
+ //std::cout << "slider e:" << speedError << " + " << positionError << std::endl;
+ currentCmd = this->pids[count]->UpdatePid(positionError + 0.0*speedError, currentTime-this->lastTime);
- sjoint->SetParam( dParamVel , currentCmd );
- sjoint->SetParam( dParamFMax, this->myIface->data->actuators[count].saturationTorque );
- break;
+ sjoint->SetParam( dParamVel , currentCmd );
+ sjoint->SetParam( dParamFMax, this->myIface->data->actuators[count].saturationTorque );
+ break;
case PR2::SPEED_CONTROL :
- sjoint->SetParam( dParamVel, cmdSpeed);
- sjoint->SetParam( dParamFMax,this->myIface->data->actuators[count].saturationTorque );
+ sjoint->SetParam( dParamVel, cmdSpeed);
+ sjoint->SetParam( dParamFMax,this->myIface->data->actuators[count].saturationTorque );
break;
default:
@@ -323,73 +325,71 @@
switch(this->myIface->data->actuators[count].controlMode)
{
case PR2::TORQUE_CONTROL:
- printf("Hinge Torque Control\n");
- hjoint->SetTorque(this->myIface->data->actuators[count].cmdEffectorForce);
- //std::cout << count << " " << this->myIface->data->actuators[count].controlMode << std::endl;
- break;
+ printf("Hinge Torque Control\n");
+ hjoint->SetTorque(this->myIface->data->actuators[count].cmdEffectorForce);
+ //std::cout << count << " " << this->myIface->data->actuators[count].controlMode << std::endl;
+ break;
case PR2::PD_TORQUE_CONTROL :
- currentAngle = hjoint->GetAngle();
+ currentAngle = hjoint->GetAngle();
// No fancy controller, just pass the commanded torque/force in (we are not modeling the motors for now)
positionError = ModNPi2Pi(cmdPosition - currentAngle);
speedError = cmdSpeed - hjoint->GetAngleRate();
currentCmd = this->pids[count]->UpdatePid(positionError + 0.0*speedError, currentTime-this->lastTime);
-// if(count==PR2::ARM_R_SHOULDER_PITCH ) std::cout << "hinge err:" << positionError << " cmd: " << currentCmd << std::endl;
- //Write out data
- if(count==PR2::ARM_R_SHOULDER_PITCH ) std::cout << currentTime<<" "<<cmdPosition<<" "<<currentAngle<<" "<<positionError<< " "<<currentCmd << std::endl;
+ // if(count==PR2::ARM_R_SHOULDER_PITCH ) std::cout << "hinge err:" << positionError << " cmd: " << currentCmd << std::endl;
+ //Write out data
+ if(count==PR2::ARM_R_SHOULDER_PITCH ) std::cout << currentTime<<" "<<cmdPosition<<" "<<currentAngle<<" "<<positionError<< " "<<currentCmd << std::endl;
// limit torque
- currentCmd = (currentCmd > 100) ? 100: currentCmd;
+ currentCmd = (currentCmd > 100) ? 100: currentCmd;
currentCmd = (currentCmd < -100 ) ? -100: currentCmd;
-/*
- currentCmd = (currentCmd > this->myIface->data->actuators[count].saturationTorque ) ? this->myIface->data->actuators[count].saturationTorque : currentCmd;
- currentCmd = (currentCmd < -this->myIface->data->actuators[count].saturationTorque ) ? -this->myIface->data->actuators[count].saturationTorque : currentCmd;
-*/
- hjoint->SetParam( dParamFMax, 0);
- hjoint->SetTorque(currentCmd);
+ //currentCmd = (currentCmd > this->myIface->data->actuators[count].saturationTorque ) ? this->myIface->data->actuators[count].saturationTorque : currentCmd;
+ //currentCmd = (currentCmd < -this->myIface->data->actuators[count].saturationTorque ) ? -this->myIface->data->actuators[count].saturationTorque : currentCmd;
+ hjoint->SetParam( dParamFMax, 0);
+ hjoint->SetTorque(currentCmd);
break;
- case PR2::SPEED_TORQUE_CONTROL :
- currentRate = hjoint->GetAngleRate();
+ case PR2::SPEED_TORQUE_CONTROL :
+ currentRate = hjoint->GetAngleRate();
speedError = cmdSpeed - currentRate;
currentCmd = this->pids[count]->UpdatePid(speedError, currentTime-this->lastTime);
- //if(count==PR2::ARM_R_PAN)std::cout<<"Joint:" <<count << " Desired:" << cmdSpeed << " Current speed"<<currentRate<<" Error"<<speedError<<" cmd: " << currentCmd << std::endl;
- if(count==PR2::ARM_R_ELBOW_PITCH )std::cout<<currentTime<<" "<<currentRate<<" "<<speedError<<" " << currentCmd << std::endl;
+ //if(count==PR2::ARM_R_PAN)std::cout<<"Joint:" <<count << " Desired:" << cmdSpeed << " Current speed"<<currentRate<<" Error"<<speedError<<" cmd: " << currentCmd << std::endl;
+ if(count==PR2::ARM_R_ELBOW_PITCH )std::cout<<currentTime<<" "<<currentRate<<" "<<speedError<<" " << currentCmd << std::endl;
// limit torque
-
+
currentCmd = (currentCmd > 100) ? 100: currentCmd;
currentCmd = (currentCmd < -100 ) ? -100: currentCmd;
-
- //Needs to be set to 0 1x for every joint
- hjoint->SetParam( dParamFMax, 0);
+
+ //Needs to be set to 0 1x for every joint
+ hjoint->SetParam( dParamFMax, 0);
hjoint->SetTorque(currentCmd);
-
+
break;
case PR2::PD_CONTROL:
- //if (cmdPosition > hjoint->GetHighStop())
- // cmdPosition = hjoint->GetHighStop();
- //else if (cmdPosition < hjoint->GetLowStop())
- // cmdPosition = hjoint->GetLowStop();
-
- positionError = ModNPi2Pi(cmdPosition - hjoint->GetAngle());
- speedError = cmdSpeed - hjoint->GetAngleRate();
- //std::cout << "hinge e:" << speedError << " + " << positionError << std::endl;
- currentCmd = this->pids[count]->UpdatePid(positionError + 0.0*speedError, currentTime-this->lastTime);
- hjoint->SetParam( dParamVel, currentCmd );
- hjoint->SetParam( dParamFMax,this->myIface->data->actuators[count].saturationTorque );
- break;
+ //if (cmdPosition > hjoint->GetHighStop())
+ // cmdPosition = hjoint->GetHighStop();
+ //else if (cmdPosition < hjoint->GetLowStop())
+ // cmdPosition = hjoint->GetLowStop();
+
+ positionError = ModNPi2Pi(cmdPosition - hjoint->GetAngle());
+ speedError = cmdSpeed - hjoint->GetAngleRate();
+ //std::cout << "hinge e:" << speedError << " + " << positionError << std::endl;
+ currentCmd = this->pids[count]->UpdatePid(positionError + 0.0*speedError, currentTime-this->lastTime);
+ hjoint->SetParam( dParamVel, currentCmd );
+ hjoint->SetParam( dParamFMax,this->myIface->data->actuators[count].saturationTorque );
+ break;
case PR2::SPEED_CONTROL:
-// printf("Hinge Speed Control\n");
- //std::cout << "wheel drive: " << cmdSpeed << " i " << count << std::endl;
- hjoint->SetParam( dParamVel, cmdSpeed);
- hjoint->SetParam( dParamFMax, this->myIface->data->actuators[count].saturationTorque );
- break;
- case PR2::DISABLED:
- hjoint->SetParam( dParamFMax, 0);
- hjoint->SetTorque(0); //disable the joint
- break;
+ // printf("Hinge Speed Control\n");
+ //std::cout << "wheel drive: " << cmdSpeed << " i " << count << std::endl;
+ hjoint->SetParam( dParamVel, cmdSpeed);
+ hjoint->SetParam( dParamFMax, this->myIface->data->actuators[count].saturationTorque );
+ break;
+ case PR2::DISABLED:
+ hjoint->SetParam( dParamFMax, 0);
+ hjoint->SetTorque(0); //disable the joint
+ break;
default:
- break;
+ break;
}
this->myIface->data->actuators[count].actualPosition = hjoint->GetAngle();
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <tf...@us...> - 2008-07-03 01:00:02
|
Revision: 1235
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=1235&view=rev
Author: tfoote
Date: 2008-07-02 17:59:57 -0700 (Wed, 02 Jul 2008)
Log Message:
-----------
renaming quaternion3D to pose3D and pose3DCache, and reorganizing, and going to CMake
Modified Paths:
--------------
pkg/trunk/exec_trex/ROSNode.cc
pkg/trunk/nav/wavefront_player/wavefront_player.cc
pkg/trunk/util/transforms/libTF/Makefile
pkg/trunk/util/transforms/libTF/include/libTF/libTF.h
Added Paths:
-----------
pkg/trunk/util/transforms/libTF/CMakeLists.txt
pkg/trunk/util/transforms/libTF/include/libTF/Pose3D.h
pkg/trunk/util/transforms/libTF/include/libTF/Pose3DCache.h
pkg/trunk/util/transforms/libTF/src/Pose3D.cpp
pkg/trunk/util/transforms/libTF/src/Pose3DCache.cpp
pkg/trunk/util/transforms/libTF/src/libTF.cpp
pkg/trunk/util/transforms/libTF/src/test/testtf.cc
Removed Paths:
-------------
pkg/trunk/util/transforms/libTF/include/libTF/Quaternion3D.h
pkg/trunk/util/transforms/libTF/src/simple/
pkg/trunk/util/transforms/libTF/src/test/Makefile
pkg/trunk/util/transforms/libTF/testtf.cc
Modified: pkg/trunk/exec_trex/ROSNode.cc
===================================================================
--- pkg/trunk/exec_trex/ROSNode.cc 2008-07-03 00:43:14 UTC (rev 1234)
+++ pkg/trunk/exec_trex/ROSNode.cc 2008-07-03 00:59:57 UTC (rev 1235)
@@ -315,9 +315,9 @@
} catch(libTF::TransformReference::LookupException& ex) {
debugMsg("ROSNode::VS", "no global->local Tx yet");
return NULL;
- } catch(libTF::Quaternion3D::ExtrapolateException& ex) {
+ } catch(libTF::Pose3DCache::ExtrapolateException& ex) {
debugMsg("ROSNode::VS",
- "libTF::Quaternion3D::ExtrapolateException occured");
+ "libTF::Pose3DCache::ExtrapolateException occured");
}
@@ -567,7 +567,7 @@
delete[] pts.pts;
return;
}
- catch(libTF::Quaternion3D::ExtrapolateException& ex)
+ catch(libTF::Pose3DCache::ExtrapolateException& ex)
{
puts("extrapolation required");
delete[] pts.pts;
Modified: pkg/trunk/nav/wavefront_player/wavefront_player.cc
===================================================================
--- pkg/trunk/nav/wavefront_player/wavefront_player.cc 2008-07-03 00:43:14 UTC (rev 1234)
+++ pkg/trunk/nav/wavefront_player/wavefront_player.cc 2008-07-03 00:59:57 UTC (rev 1235)
@@ -456,7 +456,7 @@
delete[] pts.pts;
return;
}
- catch(libTF::Quaternion3D::ExtrapolateException& ex)
+ catch(libTF::Pose3DCache::ExtrapolateException& ex)
{
//puts("extrapolation required");
delete[] pts.pts;
@@ -599,7 +599,7 @@
this->stopRobot();
return;
}
- catch(libTF::Quaternion3D::ExtrapolateException& ex)
+ catch(libTF::Pose3DCache::ExtrapolateException& ex)
{
// this should never happen
puts("WARNING: extrapolation failed!");
Added: pkg/trunk/util/transforms/libTF/CMakeLists.txt
===================================================================
--- pkg/trunk/util/transforms/libTF/CMakeLists.txt (rev 0)
+++ pkg/trunk/util/transforms/libTF/CMakeLists.txt 2008-07-03 00:59:57 UTC (rev 1235)
@@ -0,0 +1,9 @@
+cmake_minimum_required(VERSION 2.6)
+include(rosbuild)
+rospack(libTF)
+add_definitions(-Wall)
+include_directories(${libTF_INCLUDE_DIRS})
+rospack_add_library(TF src/libTF.cpp src/Pose3DCache.cpp src/Pose3D.cpp)
+link_directories(${libTF_LIBRARY_DIRS})
+rospack_add_executable(test src/test/main.cpp)
+rospack_add_executable(testtf src/test/testtf.cc)
Modified: pkg/trunk/util/transforms/libTF/Makefile
===================================================================
--- pkg/trunk/util/transforms/libTF/Makefile 2008-07-03 00:43:14 UTC (rev 1234)
+++ pkg/trunk/util/transforms/libTF/Makefile 2008-07-03 00:59:57 UTC (rev 1235)
@@ -1,29 +1 @@
-PKG = libTF
-
-CFLAGS = -g -Wall -Iinclude `rospack export/cpp/cflags $(PKG)`
-
-LDFLAGS = `rospack export/cpp/lflags $(PKG)`
-
-LIB = lib/libTF.a
-
-all: $(LIB) test
-
-
-Quaternion3D.o: src/simple/Quaternion3D.cpp include/libTF/Quaternion3D.h
- g++ $(CFLAGS) -o $@ -c $<
-
-libTF.o: src/simple/libTF.cpp include/libTF/libTF.h
- g++ $(CFLAGS) -o $@ -c $<
-
-$(LIB): libTF.o Quaternion3D.o
- ar -rs $@ $^
-
-test: src/test/main.cpp $(LIB)
- g++ $(CFLAGS) -o $@ $^ $(LDFLAGS)
-
-clean:
- rm -rf test testtf
- rm -f *.o *.a lib/*.a
-
-testtf: testtf.cc $(LIB)
- g++ $(CFLAGS) -o $@ $^ $(LDFLAGS)
\ No newline at end of file
+include $(shell rospack find mk)/cmake.mk
Copied: pkg/trunk/util/transforms/libTF/include/libTF/Pose3D.h (from rev 1232, pkg/trunk/util/transforms/libTF/include/libTF/Quaternion3D.h)
===================================================================
--- pkg/trunk/util/transforms/libTF/include/libTF/Pose3D.h (rev 0)
+++ pkg/trunk/util/transforms/libTF/include/libTF/Pose3D.h 2008-07-03 00:59:57 UTC (rev 1235)
@@ -0,0 +1,124 @@
+// Software License Agreement (BSD License)
+//
+// Copyright (c) 2008, Willow Garage, Inc.
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions
+// are met:
+//
+// * Redistributions of source code must retain the above copyright
+// notice, this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above
+// copyright notice, this list of conditions and the following
+// disclaimer in the documentation and/or other materials provided
+// with the distribution.
+// * Neither the name of the Willow Garage nor the names of its
+// contributors may be used to endorse or promote products derived
+// from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+// COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+// INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+// BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+// LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+// ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//
+#ifndef POSE3D_HH
+#define POSE3D_HH
+
+#include <iostream>
+#include <newmat10/newmat.h>
+#include <newmat10/newmatio.h>
+#include <cmath>
+#include <pthread.h>
+
+
+namespace libTF{
+
+
+ class Pose3D
+ {
+ public:
+ struct Position
+ {
+ double x,y,z;
+ };
+
+ struct Quaternion
+ {
+ double x,y,z,w;
+ };
+
+ struct Euler
+ {
+ double yaw, pitch, roll;
+ };
+
+ /* Constructors */
+ ///Empty Constructor initialize to zero
+ Pose3D();
+ ///Translation only constructor
+ Pose3D(double xt, double yt, double zt);
+ /// Quaternion only constructor
+ Pose3D(double xr, double yt, double zt, double w);
+ /// Trans and Quat constructor
+ Pose3D(double xt, double yt, double zt,
+ double xr, double yt, double zt, double w);
+
+ // Utility functions to normalize and get magnitude.
+ void Normalize();
+ double getMagnitude();
+ Pose3D & operator=(const Pose3D & input);
+
+
+ /* accessors */
+ NEWMAT::Matrix asMatrix();
+ NEWMAT::Matrix getInverseMatrix();
+ Quaternion asQuaternion();
+ Position asPosition();
+
+
+ /** Mutators **/
+ //Set the values from a matrix
+ void setFromMatrix(const NEWMAT::Matrix& matIn);
+ // Set the values using Euler angles
+ void setFromEuler(double _x, double _y, double _z, double _yaw, double _pitch, double _roll);
+ // Set the values using DH Parameters
+ void setFromDH(double length, double alpha, double offset, double theta);
+
+ /* Internal Data */
+ double xt, yt, zt, xr, yr, zr, w;
+
+
+
+
+
+
+
+ /**************** Static Helper Functions ***********************/
+ // Convert DH Parameters to a Homogeneous Transformation Matrix
+ static NEWMAT::Matrix matrixFromDH(double length, double alpha, double offset, double theta);
+ // Convert Euler Angles to a Homogeneous Transformation Matrix
+ static NEWMAT::Matrix matrixFromEuler(double ax,
+ double ay, double az, double yaw,
+ double pitch, double roll);
+
+ static Euler eulerFromMatrix(const NEWMAT::Matrix & matrix_in, unsigned int solution_number=1);
+ static Position positionFromMatrix(const NEWMAT::Matrix & matrix_in);
+
+
+ };
+
+
+
+}
+
+
+#endif //POSE3D_HH
Added: pkg/trunk/util/transforms/libTF/include/libTF/Pose3DCache.h
===================================================================
--- pkg/trunk/util/transforms/libTF/include/libTF/Pose3DCache.h (rev 0)
+++ pkg/trunk/util/transforms/libTF/include/libTF/Pose3DCache.h 2008-07-03 00:59:57 UTC (rev 1235)
@@ -0,0 +1,183 @@
+// Software License Agreement (BSD License)
+//
+// Copyright (c) 2008, Willow Garage, Inc.
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions
+// are met:
+//
+// * Redistributions of source code must retain the above copyright
+// notice, this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above
+// copyright notice, this list of conditions and the following
+// disclaimer in the documentation and/or other materials provided
+// with the distribution.
+// * Neither the name of the Willow Garage nor the names of its
+// contributors may be used to endorse or promote products derived
+// from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+// COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+// INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+// BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+// LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+// ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//
+#ifndef POSE3DCACHE_HH
+#define POSE3DCACHE_HH
+
+
+#include <iostream>
+#include <newmat10/newmat.h>
+#include <newmat10/newmatio.h>
+#include <cmath>
+#include <pthread.h>
+
+#include "libTF/Pose3D.h"
+
+namespace libTF{
+
+
+ class Pose3DCache {
+
+ public:
+ static const int MIN_INTERPOLATION_DISTANCE = 5; //!< Number of nano-seconds to not interpolate below.
+ static const unsigned int MAX_LENGTH_LINKED_LIST = 1000000; //!< Maximum length of linked list, to make sure not to be able to use unlimited memory.
+ static const unsigned long long DEFAULT_MAX_STORAGE_TIME = 10ULL * 1000000000ULL; //!< default value of 10 seconds storage
+ static const unsigned long long DEFAULT_MAX_EXTRAPOLATION_TIME = 10000000000ULL; //!< default max extrapolation of 10 seconds
+ // Storage class
+ class Pose3DStorage : public Pose3D
+ {
+ public:
+ Pose3DStorage & operator=(const Pose3DStorage & input);
+ unsigned long long time; //!<nanoseconds since 1970
+ };
+
+ /** \brief An exception class to notify that the requested value would have required extrapolation, and extrapolation is not allowed.
+ *
+ */
+ class ExtrapolateException : public std::exception
+ {
+ public:
+ virtual const char* what() const throw() { return "Disallowed extrapolation required."; }
+ };
+
+ /** Constructors **/
+ // Standard constructor max_cache_time is how long to cache transform data
+ Pose3DCache(bool interpolating = true,
+ unsigned long long max_cache_time = DEFAULT_MAX_STORAGE_TIME,
+ unsigned long long max_extrapolation_time = DEFAULT_MAX_EXTRAPOLATION_TIME);
+ ~Pose3DCache();
+
+ /** Mutators **/
+ // Set the values manually
+ void addFromQuaternion(double _xt, double _yt, double _zt, double _xr, double _yr, double _zr, double _w, unsigned long long time);
+ //Set the values from a matrix
+ void addFromMatrix(const NEWMAT::Matrix& matIn, unsigned long long time);
+ // Set the values using Euler angles
+ void addFromEuler(double _x, double _y, double _z, double _yaw, double _pitch, double _roll, unsigned long long time);
+ // Set the values using DH Parameters
+ void addFromDH(double length, double alpha, double offset, double theta, unsigned long long time);
+
+
+ /* Interpolated Accessors */
+ /** \breif Return a Pose
+ * \param time The desired time for the transformation */
+ Pose3DStorage getPoseStorage(unsigned long long time);
+ /** \brief Return the transform as a Matrix
+ * \param time The desired time for the transformation */
+ NEWMAT::Matrix getMatrix(unsigned long long time);
+ /** \breif Return the inverse matrix
+ * \param time The desired time for the transformation */
+ NEWMAT::Matrix getInverseMatrix(unsigned long long time);
+
+ /** \brief Print the stored value at a time as a matrix */
+ void printMatrix(unsigned long long time); //Not a critical part either
+ /** \brief Print the value of a specific storage unit */
+ void printStorage(const Pose3DStorage &storage); //Do i need this now that i've got the ostream method??
+
+ /** \brief Remove all nodes from the list
+ * This will clear all cached transformations. */
+ void clearList();
+
+
+ private:
+ /**** Linked List stuff ****/
+
+ struct data_LL;
+ /// The data structure for the linked list
+ struct data_LL{
+ Pose3DStorage data;
+ struct data_LL * next;
+ struct data_LL * previous;
+ };
+ /** \brief The internal method for getting data out of the linked list */
+ bool getValue(Pose3DStorage& buff, unsigned long long time, long long &time_diff);
+ /** \brief The internal method for adding to the linked list
+ * by all the specific add functions. */
+ void add_value(const Pose3DStorage&);
+
+
+ ExtrapolateException MaxExtrapolation;
+
+ /** \brief insert a node into the sorted linked list */
+ void insertNode(const Pose3DStorage & );
+
+ /** \brief prune data older than max_storage_time from the list */
+ void pruneList();
+
+ /** \brief Find the closest two points in the list
+ *Return the distance to the closest one*/
+ int findClosest(Pose3DStorage& one, Pose3DStorage& two, const unsigned long long target_time, long long &time_diff);
+
+ /** \brief Interpolate between two nodes and return the interpolated value
+ * This must always take two valid points!!!!!! */
+ void interpolate(const Pose3DStorage &one, const Pose3DStorage &two, const unsigned long long target_time, Pose3DStorage& output);
+
+ /** Linearly interpolate two doubles
+ *Used by interpolate to interpolate between double values */
+ double interpolateDouble(const double, const unsigned long long, const double, const unsigned long long, const unsigned long long);
+
+ ///How long to cache incoming values
+ unsigned long long max_storage_time;
+ ///Max length of linked list
+ unsigned long long max_length_linked_list;
+ ///Whether to allow extrapolation
+ unsigned long long max_extrapolation_time;
+
+ ///A mutex to prevent linked list collisions
+ pthread_mutex_t linked_list_mutex;
+
+ ///Pointer for the start of a sorted linked list.
+ data_LL* first;
+ ///Pointer for the end of a sorted linked list.
+ data_LL* last;
+
+ ///The length of the list
+ unsigned int list_length;
+
+ ///Whether or not to interpolate
+ bool interpolating;
+
+ };
+
+
+ /** \brief A namespace ostream overload for displaying storage */
+ std::ostream & operator<<(std::ostream& mystream,const Pose3DCache::Pose3DStorage & storage);
+
+};
+
+
+
+
+
+
+
+#endif //POSE3DCACHE_HH
Property changes on: pkg/trunk/util/transforms/libTF/include/libTF/Pose3DCache.h
___________________________________________________________________
Name: svn:mime-type
+ text/cpp
Deleted: pkg/trunk/util/transforms/libTF/include/libTF/Quaternion3D.h
===================================================================
--- pkg/trunk/util/transforms/libTF/include/libTF/Quaternion3D.h 2008-07-03 00:43:14 UTC (rev 1234)
+++ pkg/trunk/util/transforms/libTF/include/libTF/Quaternion3D.h 2008-07-03 00:59:57 UTC (rev 1235)
@@ -1,257 +0,0 @@
-// Software License Agreement (BSD License)
-//
-// Copyright (c) 2008, Willow Garage, Inc.
-// All rights reserved.
-//
-// Redistribution and use in source and binary forms, with or without
-// modification, are permitted provided that the following conditions
-// are met:
-//
-// * Redistributions of source code must retain the above copyright
-// notice, this list of conditions and the following disclaimer.
-// * Redistributions in binary form must reproduce the above
-// copyright notice, this list of conditions and the following
-// disclaimer in the documentation and/or other materials provided
-// with the distribution.
-// * Neither the name of the Willow Garage nor the names of its
-// contributors may be used to endorse or promote products derived
-// from this software without specific prior written permission.
-//
-// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-// COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-// INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-// BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
-// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
-// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-// LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-// ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-// POSSIBILITY OF SUCH DAMAGE.
-//
-#ifndef QUATERNION3D_HH
-#define QUATERNION3D_HH
-
-#include <iostream>
-#include <newmat10/newmat.h>
-#include <newmat10/newmatio.h>
-#include <math.h>
-#include <pthread.h>
-
-
-namespace libTF{
-
-
-class Pose3D
-{
- public:
- struct Position
- {
- double x,y,z;
- };
-
- struct Quaternion
- {
- double x,y,z,w;
- };
-
- struct Euler
- {
- double yaw, pitch, roll;
- };
-
- /* Constructors */
- ///Empty Constructor initialize to zero
- Pose3D();
- ///Translation only constructor
- Pose3D(double xt, double yt, double zt);
- /// Quaternion only constructor
- Pose3D(double xr, double yt, double zt, double w);
- /// Trans and Quat constructor
- Pose3D(double xt, double yt, double zt,
- double xr, double yt, double zt, double w);
-
- // Utility functions to normalize and get magnitude.
- void Normalize();
- double getMagnitude();
- Pose3D & operator=(const Pose3D & input);
-
-
- /* accessors */
- NEWMAT::Matrix asMatrix();
- NEWMAT::Matrix getInverseMatrix();
- Quaternion asQuaternion();
- Position asPosition();
-
-
- /** Mutators **/
- //Set the values from a matrix
- void setFromMatrix(const NEWMAT::Matrix& matIn);
- // Set the values using Euler angles
- void setFromEuler(double _x, double _y, double _z, double _yaw, double _pitch, double _roll);
- // Set the values using DH Parameters
- void setFromDH(double length, double alpha, double offset, double theta);
-
- /* Internal Data */
- double xt, yt, zt, xr, yr, zr, w;
-
-
-
-
-
-
-
- /**************** Static Helper Functions ***********************/
- // Convert DH Parameters to a Homogeneous Transformation Matrix
- static NEWMAT::Matrix matrixFromDH(double length, double alpha, double offset, double theta);
- // Convert Euler Angles to a Homogeneous Transformation Matrix
- static NEWMAT::Matrix matrixFromEuler(double ax,
- double ay, double az, double yaw,
- double pitch, double roll);
-
- static Euler eulerFromMatrix(const NEWMAT::Matrix & matrix_in, unsigned int solution_number=1);
- static Position positionFromMatrix(const NEWMAT::Matrix & matrix_in);
-
-
-};
-
-
-
-
-class Pose3DCache {
-
-public:
- static const int MIN_INTERPOLATION_DISTANCE = 5; //!< Number of nano-seconds to not interpolate below.
- static const unsigned int MAX_LENGTH_LINKED_LIST = 1000000; //!< Maximum length of linked list, to make sure not to be able to use unlimited memory.
- static const unsigned long long DEFAULT_MAX_STORAGE_TIME = 10ULL * 1000000000ULL; //!< default value of 10 seconds storage
- static const unsigned long long DEFAULT_MAX_EXTRAPOLATION_TIME = 10000000000ULL; //!< default max extrapolation of 10 seconds
- // Storage class
- class Pose3DStorage : public Pose3D
- {
- public:
- Pose3DStorage & operator=(const Pose3DStorage & input);
- unsigned long long time; //!<nanoseconds since 1970
- };
-
- /** \brief An exception class to notify that the requested value would have required extrapolation, and extrapolation is not allowed.
- *
- */
- class ExtrapolateException : public std::exception
- {
- public:
- virtual const char* what() const throw() { return "Disallowed extrapolation required."; }
- };
-
- /** Constructors **/
- // Standard constructor max_cache_time is how long to cache transform data
- Pose3DCache(bool interpolating = true,
- unsigned long long max_cache_time = DEFAULT_MAX_STORAGE_TIME,
- unsigned long long max_extrapolation_time = DEFAULT_MAX_EXTRAPOLATION_TIME);
- ~Pose3DCache();
-
- /** Mutators **/
- // Set the values manually
- void addFromQuaternion(double _xt, double _yt, double _zt, double _xr, double _yr, double _zr, double _w, unsigned long long time);
- //Set the values from a matrix
- void addFromMatrix(const NEWMAT::Matrix& matIn, unsigned long long time);
- // Set the values using Euler angles
- void addFromEuler(double _x, double _y, double _z, double _yaw, double _pitch, double _roll, unsigned long long time);
- // Set the values using DH Parameters
- void addFromDH(double length, double alpha, double offset, double theta, unsigned long long time);
-
-
- /* Interpolated Accessors */
- /** \breif Return a Pose
- * \param time The desired time for the transformation */
- Pose3DStorage getPoseStorage(unsigned long long time);
- /** \brief Return the transform as a Matrix
- * \param time The desired time for the transformation */
- NEWMAT::Matrix getMatrix(unsigned long long time);
- /** \breif Return the inverse matrix
- * \param time The desired time for the transformation */
- NEWMAT::Matrix getInverseMatrix(unsigned long long time);
-
- /** \brief Print the stored value at a time as a matrix */
- void printMatrix(unsigned long long time); //Not a critical part either
- /** \brief Print the value of a specific storage unit */
- void printStorage(const Pose3DStorage &storage); //Do i need this now that i've got the ostream method??
-
- /** \brief Remove all nodes from the list
- * This will clear all cached transformations. */
- void clearList();
-
-
-private:
- /**** Linked List stuff ****/
-
- struct data_LL;
- /// The data structure for the linked list
- struct data_LL{
- Pose3DStorage data;
- struct data_LL * next;
- struct data_LL * previous;
- };
- /** \brief The internal method for getting data out of the linked list */
- bool getValue(Pose3DStorage& buff, unsigned long long time, long long &time_diff);
- /** \brief The internal method for adding to the linked list
- * by all the specific add functions. */
- void add_value(const Pose3DStorage&);
-
-
- ExtrapolateException MaxExtrapolation;
-
- /** \brief insert a node into the sorted linked list */
- void insertNode(const Pose3DStorage & );
-
- /** \brief prune data older than max_storage_time from the list */
- void pruneList();
-
- /** \brief Find the closest two points in the list
- *Return the distance to the closest one*/
- int findClosest(Pose3DStorage& one, Pose3DStorage& two, const unsigned long long target_time, long long &time_diff);
-
- /** \brief Interpolate between two nodes and return the interpolated value
- * This must always take two valid points!!!!!! */
- void interpolate(const Pose3DStorage &one, const Pose3DStorage &two, const unsigned long long target_time, Pose3DStorage& output);
-
- /** Linearly interpolate two doubles
- *Used by interpolate to interpolate between double values */
- double interpolateDouble(const double, const unsigned long long, const double, const unsigned long long, const unsigned long long);
-
- ///How long to cache incoming values
- unsigned long long max_storage_time;
- ///Max length of linked list
- unsigned long long max_length_linked_list;
- ///Whether to allow extrapolation
- unsigned long long max_extrapolation_time;
-
- ///A mutex to prevent linked list collisions
- pthread_mutex_t linked_list_mutex;
-
- ///Pointer for the start of a sorted linked list.
- data_LL* first;
- ///Pointer for the end of a sorted linked list.
- data_LL* last;
-
- ///The length of the list
- unsigned int list_length;
-
- ///Whether or not to interpolate
- bool interpolating;
-
-};
-
-
-/** \brief A namespace ostream overload for displaying storage */
-std::ostream & operator<<(std::ostream& mystream,const Pose3DCache::Pose3DStorage & storage);
-
-};
-
-
-
-
-
-
-
-#endif //QUATERNION3D_HH
Modified: pkg/trunk/util/transforms/libTF/include/libTF/libTF.h
===================================================================
--- pkg/trunk/util/transforms/libTF/include/libTF/libTF.h 2008-07-03 00:43:14 UTC (rev 1234)
+++ pkg/trunk/util/transforms/libTF/include/libTF/libTF.h 2008-07-03 00:59:57 UTC (rev 1235)
@@ -36,11 +36,11 @@
#include <iomanip>
#include <newmat10/newmat.h>
#include <newmat10/newmatio.h>
-#include <math.h>
+#include <cmath>
#include <vector>
#include <sstream>
-#include "Quaternion3D.h"
+#include "libTF/Pose3DCache.h"
/** \mainpage libTF a transformation library
*
@@ -401,5 +401,5 @@
NEWMAT::Matrix computeTransformFromList(const TransformLists & list, ULLtime time);
};
-};
+}
#endif //LIBTF_HH
Copied: pkg/trunk/util/transforms/libTF/src/Pose3D.cpp (from rev 1232, pkg/trunk/util/transforms/libTF/src/simple/Quaternion3D.cpp)
===================================================================
--- pkg/trunk/util/transforms/libTF/src/Pose3D.cpp (rev 0)
+++ pkg/trunk/util/transforms/libTF/src/Pose3D.cpp 2008-07-03 00:59:57 UTC (rev 1235)
@@ -0,0 +1,363 @@
+// Software License Agreement (BSD License)
+//
+// Copyright (c) 2008, Willow Garage, Inc.
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions
+// are met:
+//
+// * Redistributions of source code must retain the above copyright
+// notice, this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above
+// copyright notice, this list of conditions and the following
+// disclaimer in the documentation and/or other materials provided
+// with the distribution.
+// * Neither the name of the Willow Garage nor the names of its
+// contributors may be used to endorse or promote products derived
+// from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+// COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+// INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+// BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+// LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+// ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//
+#include "libTF/Pose3D.h"
+
+#include <cassert>
+
+using namespace libTF;
+
+Pose3D::Pose3D():
+ xt(0), yt(0),zt(0),
+ xr(1), yr(0), zr(0),w(0)
+{
+};
+
+
+///Translation only constructor
+Pose3D::Pose3D(double xt, double yt, double zt):
+ xt(xt), yt(yt),zt(zt),
+ xr(1), yr(0), zr(0),w(0)
+{
+};
+/// Quaternion only constructor
+Pose3D::Pose3D(double xr, double yr, double zr, double w):
+ xt(0), yt(0),zt(0),
+ xr(xr), yr(yr), zr(zr),w(w)
+{
+};
+ /// Trans and Quat constructor
+Pose3D::Pose3D(double xt, double yt, double zt,
+ double xr, double yr, double zr, double w):
+ xt(xt), yt(yt),zt(zt),
+ xr(xr), yr(yr), zr(zr),w(w)
+{
+};
+
+
+void Pose3D::setFromMatrix(const NEWMAT::Matrix& matIn)
+{
+ // math derived from http://www.j3d.org/matrix_faq/matrfaq_latest.html
+
+ // std::cout <<std::endl<<"Matrix in"<<std::endl<<matIn;
+
+ const double * mat = matIn.Store();
+ //Get the translations
+ xt = mat[3];
+ yt = mat[7];
+ zt = ...
[truncated message content] |
|
From: <mor...@us...> - 2008-07-03 08:59:36
|
Revision: 1246
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=1246&view=rev
Author: morgan_quigley
Date: 2008-07-03 01:23:55 -0700 (Thu, 03 Jul 2008)
Log Message:
-----------
ruby script hacking to slap together demos
Added Paths:
-----------
pkg/trunk/highlevel/
pkg/trunk/highlevel/readme
pkg/trunk/highlevel/rubot/
pkg/trunk/highlevel/rubot/manifest.xml
pkg/trunk/highlevel/rubot/ronsole
pkg/trunk/highlevel/rubot/rubot/
pkg/trunk/highlevel/rubot/rubot/rubot.rb
pkg/trunk/highlevel/rubot/rubot/stair1.rb
pkg/trunk/highlevel/rubot/test/
pkg/trunk/highlevel/rubot/test/assimilate
pkg/trunk/highlevel/rubot/test/platform_stage.xml
pkg/trunk/highlevel/rubot/test/simple
Added: pkg/trunk/highlevel/readme
===================================================================
--- pkg/trunk/highlevel/readme (rev 0)
+++ pkg/trunk/highlevel/readme 2008-07-03 08:23:55 UTC (rev 1246)
@@ -0,0 +1,3 @@
+this directory is meant to contain high-level stuff, i.e. software that views
+the robot in very abstract terms, like "drive to XY on a map" or
+"open the door in front of you" or "pick up the stapler in front of you"
Added: pkg/trunk/highlevel/rubot/manifest.xml
===================================================================
--- pkg/trunk/highlevel/rubot/manifest.xml (rev 0)
+++ pkg/trunk/highlevel/rubot/manifest.xml 2008-07-03 08:23:55 UTC (rev 1246)
@@ -0,0 +1,13 @@
+<package>
+ <description>
+ A rubot is a ruby-controlled abstract robot. The concept is just to have a
+ few dumb ruby files for scripting lower-level (not necessarily simple)
+ actions together to make demos. For example, "drive to XY, then open the door
+ in front of you, then drive to another XY." This is not trying to compete
+ with more sophisticated algorithsm like TR or whatever; it's just a way to
+ slap together a few actions to show them off for a demo.
+ </description>
+ <author>Morgan Quigley</author>
+ <license>BSD</license>
+</package>
+
Added: pkg/trunk/highlevel/rubot/ronsole
===================================================================
--- pkg/trunk/highlevel/rubot/ronsole (rev 0)
+++ pkg/trunk/highlevel/rubot/ronsole 2008-07-03 08:23:55 UTC (rev 1246)
@@ -0,0 +1,7 @@
+#!/usr/bin/env ruby
+$:.push(`rospack find rubot`.strip)
+require 'rubot/stair1.rb'
+puts "You are now operating a STAIR1 robot"
+r = Stair1.new
+r.console
+
Property changes on: pkg/trunk/highlevel/rubot/ronsole
___________________________________________________________________
Name: svn:executable
+ *
Added: pkg/trunk/highlevel/rubot/rubot/rubot.rb
===================================================================
--- pkg/trunk/highlevel/rubot/rubot/rubot.rb (rev 0)
+++ pkg/trunk/highlevel/rubot/rubot/rubot.rb 2008-07-03 08:23:55 UTC (rev 1246)
@@ -0,0 +1,41 @@
+require 'irb'
+require 'irb/completion'
+
+module IRB
+ def IRB.start_session(*args)
+ unless $irb
+ IRB.setup nil
+ end
+ workspace = WorkSpace.new(*args)
+ $irb = Irb.new(workspace)
+ @CONF[:MAIN_CONTEXT] = $irb.context
+ trap 'INT' do
+ $irb.signal_handle
+ end
+ catch :IRB_EXIT do
+ $irb.eval_input
+ end
+ end
+end
+
+class Rubot
+ attr_accessor :actions
+ def initialize
+ @actions = Array.new
+ end
+ def register_action name, help
+ @actions << [name, help]
+ end
+ def console
+ IRB.start_session(self)
+ end
+ def show_actions
+ puts
+ puts "Here are your actions:"
+ puts "----------------------"
+ actions.each {|a| puts a[1]}
+ puts "----------------------"
+ nil
+ end
+end
+
Added: pkg/trunk/highlevel/rubot/rubot/stair1.rb
===================================================================
--- pkg/trunk/highlevel/rubot/rubot/stair1.rb (rev 0)
+++ pkg/trunk/highlevel/rubot/rubot/stair1.rb 2008-07-03 08:23:55 UTC (rev 1246)
@@ -0,0 +1,32 @@
+require(`rospack find rubot`.strip + "/rubot/rubot.rb")
+
+class Stair1 < Rubot
+ def initialize
+ super
+ register_action('deadreckon', 'deadreckon RANGE BEARING FINALHEADING')
+ register_action('inventory', 'inventory')
+ end
+ def navigate(x, y, th)
+ puts "stair1 navigate to #{x}, #{y}, #{th}"
+ system "#{`rospack find mux`.strip}/switch mux:=velMux wavefrontVel"
+ system "#{`rospack find wavefront_player`.strip}/query_wavefront #{x} #{y} #{th}"
+ end
+ def deadreckon(range, bearing, final_heading)
+ puts "stair1 deadreckon to #{range}, #{bearing}, #{final_heading}"
+ system "#{`rospack find mux`.strip}/switch mux:=velMux deadReckonVel"
+ system "#{`rospack find deadreckon`.strip}/test_deadreckon_service #{range} #{bearing} #{final_heading}"
+ end
+ def inventory
+ puts ""
+ puts "You are carrying:"
+ puts "-----------------"
+ puts "segway"
+ puts "laser"
+ puts "manipulator"
+ puts "stereo camera"
+ puts "PTZ camera"
+ puts "mono camera"
+ puts "-----------------"
+ puts ""
+ end
+end
Added: pkg/trunk/highlevel/rubot/test/assimilate
===================================================================
--- pkg/trunk/highlevel/rubot/test/assimilate (rev 0)
+++ pkg/trunk/highlevel/rubot/test/assimilate 2008-07-03 08:23:55 UTC (rev 1246)
@@ -0,0 +1,8 @@
+#!/usr/bin/env ruby
+$:.push(`rospack find rubot`.strip)
+require 'rubot/stair1.rb'
+
+r = Stair1.new
+r.navigate(24, 19, 0.6)
+#r.deadreckon(1,2,3)
+#r.drive(4,5,6)
Property changes on: pkg/trunk/highlevel/rubot/test/assimilate
___________________________________________________________________
Name: svn:executable
+ *
Added: pkg/trunk/highlevel/rubot/test/platform_stage.xml
===================================================================
--- pkg/trunk/highlevel/rubot/test/platform_stage.xml (rev 0)
+++ pkg/trunk/highlevel/rubot/test/platform_stage.xml 2008-07-03 08:23:55 UTC (rev 1246)
@@ -0,0 +1,8 @@
+<launch>
+ <node pkg="rosstage" type="rosstage" args="$(find 2dnav-stage)/willow-erratic.world"/>
+ <node pkg="map_server" type="map_server" args="$(find 2dnav-stage)/willow-full.png 0.1"/>
+ <node pkg="amcl_player" type="amcl_player"/>
+ <node pkg="wavefront_player" type="wavefront_player cmd_vel:=wavefrontVel"/>
+ <node pkg="nav_view" type="nav_view"/>
+ <node pkg="mux" type="mux" args="cmd_vel velMux wavefrontVel deadReckonVel"/>
+</launch>
Added: pkg/trunk/highlevel/rubot/test/simple
===================================================================
--- pkg/trunk/highlevel/rubot/test/simple (rev 0)
+++ pkg/trunk/highlevel/rubot/test/simple 2008-07-03 08:23:55 UTC (rev 1246)
@@ -0,0 +1,7 @@
+#!/usr/bin/env ruby
+$:.push(`rospack find rubot`.strip)
+require 'rubot/stair1.rb'
+
+r = Stair1.new
+r.deadreckon(1,2,3)
+r.drive(4,5,6)
Property changes on: pkg/trunk/highlevel/rubot/test/simple
___________________________________________________________________
Name: svn:executable
+ *
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <hsu...@us...> - 2008-07-09 17:16:08
|
Revision: 1362
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=1362&view=rev
Author: hsujohnhsu
Date: 2008-07-09 10:16:16 -0700 (Wed, 09 Jul 2008)
Log Message:
-----------
broadcast arm frame transforms.
only pr2.model is updated, will update pr2_torque.model next.
Modified Paths:
--------------
pkg/trunk/controllers/pr2Controllers/Makefile
pkg/trunk/controllers/pr2Controllers/manifest.xml
pkg/trunk/demos/2dnav-gazebo/world/pr2.model
pkg/trunk/drivers/robot/pr2/pr2Core/include/pr2Core/pr2Core.h
pkg/trunk/simulators/pr2SimRT/manifest.xml
pkg/trunk/simulators/rosgazebo/rosgazebo.cc
Modified: pkg/trunk/controllers/pr2Controllers/Makefile
===================================================================
--- pkg/trunk/controllers/pr2Controllers/Makefile 2008-07-09 17:13:22 UTC (rev 1361)
+++ pkg/trunk/controllers/pr2Controllers/Makefile 2008-07-09 17:16:16 UTC (rev 1362)
@@ -1,5 +1,5 @@
SRC = src/BaseController.cpp src/HeadController.cpp src/SpineController.cpp src/ArmController.cpp src/LaserScannerController.cpp src/GripperController.cpp
-OUT = lib/libPr2Controllers.a
+OUT = lib/libpr2Controllers.a
PKG = pr2Controllers
include $(shell rospack find mk)/lib.mk
Modified: pkg/trunk/controllers/pr2Controllers/manifest.xml
===================================================================
--- pkg/trunk/controllers/pr2Controllers/manifest.xml 2008-07-09 17:13:22 UTC (rev 1361)
+++ pkg/trunk/controllers/pr2Controllers/manifest.xml 2008-07-09 17:16:16 UTC (rev 1362)
@@ -13,6 +13,6 @@
<url>http://pr.willowgarage.com</url>
<repository>http://pr.willowgarage.com/repos</repository>
<export>
- <cpp cflags='-I${prefix}/include' lflags='-L${prefix}/lib -lPr2Controllers'/>
+ <cpp cflags='-I${prefix}/include' lflags='-L${prefix}/lib -lpr2Controllers'/>
</export>
</package>
Modified: pkg/trunk/demos/2dnav-gazebo/world/pr2.model
===================================================================
--- pkg/trunk/demos/2dnav-gazebo/world/pr2.model 2008-07-09 17:13:22 UTC (rev 1361)
+++ pkg/trunk/demos/2dnav-gazebo/world/pr2.model 2008-07-09 17:16:16 UTC (rev 1362)
@@ -179,16 +179,18 @@
<!-- neck body -->
<canonicalBody>neck_body</canonicalBody>
<body:box name="neck_body">
- <xyz>0.10 0.0 1.08</xyz>
+ <xyz>0.10 0.0 1.03</xyz>
<rpy>0.0 0.0 0.0</rpy>
<!--xyz>0.0 0.0 0.0</xyz>
<rpy>0.0 0.0 0.0</rpy-->
<geom:box name="neck_geom">
- <size>.20 .38 .14</size>
+ <!--size>.20 .38 .14</size-->
+ <size>0.01 0.01 0.01</size>
<mass>0.01</mass>
<visual>
<mesh>unit_box</mesh>
- <size>.20 .38 .14</size>
+ <!--size>.20 .38 .14</size-->
+ <size>0.01 0.01 0.01</size>
<material>Gazebo/Black</material>
</visual>
</geom:box>
@@ -221,7 +223,7 @@
<canonicalBody>head_base_body</canonicalBody>
<body:box name="head_base_body">
- <xyz>0.10 0.0 1.18 </xyz>
+ <xyz>0.10 0.0 1.13 </xyz>
<rpy>0.0 0.0 0.0 </rpy>
<!-- report when this body collides with static bodies -->
<reportStaticCollision>true</reportStaticCollision>
@@ -242,7 +244,7 @@
<canonicalBody>head_tilt_body</canonicalBody>
<body:box name="head_tilt_body">
- <xyz>0.10 0.0 1.12 </xyz>
+ <xyz>0.10 0.0 1.07 </xyz>
<rpy>0.0 0.0 0.0 </rpy>
<!-- report when this body collides with static bodies -->
<reportStaticCollision>true</reportStaticCollision>
@@ -306,7 +308,7 @@
<!-- left camera -->
<body:box name="head_cam_left_body">
- <xyz> 0.15 0.23 1.12 </xyz>
+ <xyz> 0.15 0.23 1.07 </xyz>
<rpy> 0.00 0.0 0.00 </rpy>
<sensor:camera name="head_cam_left_sensor">
<imageSize>640 480</imageSize>
@@ -333,7 +335,7 @@
</geom:box>
</body:box>
<body:box name="head_cam_base_left_body">
- <xyz> 0.15 0.20 1.12 </xyz>
+ <xyz> 0.15 0.20 1.07 </xyz>
<rpy> 0.00 0.0 0.00 </rpy>
<geom:box name="head_cam_base_left_geom">
<xyz> 0.00 0.00 0.00</xyz>
@@ -351,7 +353,7 @@
<!-- right camera -->
<body:empty name="head_cam_right_body">
- <xyz> 0.15 -0.23 1.12 </xyz>
+ <xyz> 0.15 -0.23 1.07 </xyz>
<rpy> 0.00 0.0 0.00 </rpy>
<sensor:camera name="head_cam_right_sensor">
<imageSize>640 480</imageSize>
@@ -378,7 +380,7 @@
</geom:box>
</body:empty>
<body:box name="head_cam_base_right_body">
- <xyz> 0.15 -0.20 1.12 </xyz>
+ <xyz> 0.15 -0.20 1.07 </xyz>
<rpy> 0.00 0.0 0.00 </rpy>
<geom:box name="head_cam_base_right_geom">
<xyz> 0.00 0.00 0.00</xyz>
@@ -423,7 +425,7 @@
<!-- stereo camera -->
<body:empty name="stereo_camera_body">
- <xyz> 0.10 0.0 1.3 </xyz>
+ <xyz> 0.10 0.0 1.25 </xyz>
<rpy> 0.0 0.0 0.0 </rpy>
<sensor:stereocamera name="stereo_camera_sensor">
<imageSize>640 480</imageSize>
@@ -525,7 +527,7 @@
<canonicalBody>torso_body</canonicalBody>
<body:box name="torso_body">
- <xyz>0.056 0.0 0.5</xyz>
+ <xyz>0.056 0.0 0.45</xyz>
<rpy>0.0 0.0 0.0</rpy>
<!-- report when this body collides with static bodies -->
@@ -533,7 +535,7 @@
<geom:box name="torso_geom">
<xyz> 0.0 0.0 0.0 </xyz>
- <size>0.1 0.2 0.8 </size>
+ <size>0.1 0.2 0.6 </size>
<mass>1</mass>
<visual>
<!-- x y z -->
@@ -547,7 +549,7 @@
</geom:box>
<geom:box name="shoulder_geom">
- <xyz> 0.1 0.00 0.51</xyz>
+ <xyz> 0.1 0.00 0.54</xyz>
<size>0.3 0.65 0.02</size>
<mass>1</mass>
<visual>
@@ -571,7 +573,7 @@
<!-- ========== Right Arm =========== -->
<body:cylinder name="shoulder_pan_body_right">
- <xyz> 0.185 -0.15 0.68 </xyz>
+ <xyz> 0.1829361 -0.1329361 0.64 </xyz>
<rpy> 0.0 0.0 0.0 </rpy>
<geom:cylinder name="shoulder_pan_body_right_geom">
@@ -589,9 +591,9 @@
<kp>1000000.0</kp>
<kd>1.0</kd>
- <size> 0.13 0.6</size> <!-- radius and length -->
+ <size> 0.13 0.60</size> <!-- radius and length -->
<visual>
- <xyz> 0.00 0.0 0.20 </xyz>
+ <xyz> 0.00 0.0 0.185 </xyz>
<rpy> 90.0 0.0 90.0 </rpy>
<!--size> 0.26 0.6 0.36</size-->
<scale>0.001 0.001 0.001</scale>
@@ -602,7 +604,7 @@
</body:cylinder>
<body:cylinder name="upperarm_joint_right">
- <xyz> 0.285 -0.15 0.86 </xyz>
+ <xyz> 0.285 -0.15 0.8269 </xyz>
<rpy> 90.0 0.0 0.0 </rpy>
<geom:cylinder name="upperarm_joint_right_geom">
@@ -632,7 +634,7 @@
</body:cylinder>
<body:box name="upperarm_right">
- <xyz> 0.575 -0.15 0.86 </xyz>
+ <xyz> 0.575 -0.15 0.8269 </xyz>
<rpy> 0.0 0.0 0.0 </rpy>
<geom:box name="upperarm_right_geom">
@@ -662,7 +664,7 @@
</body:box>
<body:cylinder name="elbow_right">
- <xyz> 0.6850 -0.15 0.86 </xyz>
+ <xyz> 0.6850 -0.15 0.8269 </xyz>
<rpy> 90.0 0.0 0.0 </rpy>
<geom:cylinder name="elbow_right_geom">
@@ -692,7 +694,7 @@
</body:cylinder>
<body:box name="forearm_right">
- <xyz> 0.90225 -0.15 0.86 </xyz>
+ <xyz> 0.90225 -0.15 0.8269 </xyz>
<rpy> 0.0 0.0 0.0 </rpy>
<geom:box name="forearm_right_geom">
@@ -722,7 +724,7 @@
</body:box>
<body:cylinder name="wrist_right">
- <xyz> 1.008 -0.15 0.86 </xyz>
+ <xyz> 1.008 -0.15 0.8269 </xyz>
<rpy> 90.0 0.0 0.0 </rpy>
<geom:cylinder name="wrist_right_geom">
@@ -752,7 +754,7 @@
</body:cylinder>
<body:box name="palm_right">
- <xyz> 1.10 -0.15 0.86</xyz>
+ <xyz> 1.10 -0.15 0.8269</xyz>
<rpy> 0.0 0.0 0.0 </rpy>
<geom:box name="palm_right_geom">
@@ -782,7 +784,7 @@
</body:box>
<body:box name="finger_1_right">
- <xyz> 1.18 -0.17 0.86 </xyz>
+ <xyz> 1.18 -0.17 0.8269 </xyz>
<rpy> 0.0 0.0 0.0 </rpy>
<geom:box name="finger_1_right_geom">
<kp>100000000.0</kp>
@@ -798,7 +800,7 @@
</body:box>
<body:box name="finger_2_right">
- <xyz> 1.18 -0.13 0.86 </xyz>
+ <xyz> 1.18 -0.13 0.8269 </xyz>
<rpy> 0.0 0.0 0.0 </rpy>
<geom:box name="finger_2_right_geom">
<kp>100000000.0</kp>
@@ -888,7 +890,7 @@
<!-- ========== Left Arm =========== -->
<body:cylinder name="shoulder_pan_body_left">
- <xyz> 0.185 0.15 0.68 </xyz>
+ <xyz> 0.1829361 0.1329361 0.64 </xyz>
<rpy> 0.0 0.0 0.0 </rpy>
<geom:cylinder name="shoulder_pan_body_left_geom">
@@ -908,7 +910,7 @@
<kd>1.0</kd>
<size> 0.13 0.6</size> <!-- radius and length -->
<visual>
- <xyz> 0.00 0.0 0.20 </xyz>
+ <xyz> 0.00 0.0 0.185 </xyz>
<rpy> 90.0 0.0 90.0 </rpy>
<!--size> 0.26 0.6 0.36</size-->
<scale>0.001 0.001 0.001</scale>
@@ -919,7 +921,7 @@
</body:cylinder>
<body:cylinder name="upperarm_joint_left">
- <xyz> 0.285 0.15 0.86 </xyz>
+ <xyz> 0.285 0.15 0.8269 </xyz>
<rpy> 90.0 0.0 0.0 </rpy>
<geom:cylinder name="upperarm_joint_left_geom">
@@ -949,7 +951,7 @@
</body:cylinder>
<body:box name="upperarm_left">
- <xyz> 0.575 0.15 0.86 </xyz>
+ <xyz> 0.575 0.15 0.8269 </xyz>
<rpy> 0.0 0.0 0.0 </rpy>
<geom:box name="upperarm_left_geom">
@@ -979,7 +981,7 @@
</body:box>
<body:cylinder name="elbow_left">
- <xyz> 0.6850 0.15 0.86 </xyz>
+ <xyz> 0.6850 0.15 0.8269 </xyz>
<rpy> 90.0 0.0 0.0 </rpy>
<geom:cylinder name="elbow_left_geom">
@@ -1009,7 +1011,7 @@
</body:cylinder>
<body:box name="forearm_left">
- <xyz> 0.90225 0.15 0.86 </xyz>
+ <xyz> 0.90225 0.15 0.8269 </xyz>
<rpy> 0.0 0.0 0.0 </rpy>
<geom:box name="forearm_left_geom">
@@ -1039,7 +1041,7 @@
</body:box>
<body:cylinder name="wrist_left">
- <xyz> 1.008 0.15 0.86 </xyz>
+ <xyz> 1.008 0.15 0.8269 </xyz>
<rpy> 90.0 0.0 0.0 </rpy>
<geom:cylinder name="wrist_left_geom">
@@ -1069,7 +1071,7 @@
</body:cylinder>
<body:box name="palm_left">
- <xyz> 1.10 0.15 0.86</xyz>
+ <xyz> 1.10 0.15 0.8269</xyz>
<rpy> 0.0 0.0 0.0 </rpy>
<geom:box name="palm_left_geom">
@@ -1099,7 +1101,7 @@
</body:box>
<body:box name="finger_1_left">
- <xyz> 1.18 0.13 0.86 </xyz>
+ <xyz> 1.18 0.13 0.8269 </xyz>
<rpy> 0.0 0.0 0.0 </rpy>
<geom:box name="finger_1_left_geom">
<kp>100000000.0</kp>
@@ -1115,7 +1117,7 @@
</body:box>
<body:box name="finger_2_left">
- <xyz> 1.18 0.17 0.86 </xyz>
+ <xyz> 1.18 0.17 0.8269 </xyz>
<rpy> 0.0 0.0 0.0 </rpy>
<geom:box name="finger_2_left_geom">
<kp>100000000.0</kp>
@@ -1693,7 +1695,7 @@
<!-- Hokuyo laser body -->
<canonicalBody>laser_body</canonicalBody>
<body:box name="laser_body">
- <xyz>0.23 0.0 1.05</xyz>
+ <xyz>0.23 0.0 1.0</xyz>
<rpy>0.0 0.0 0.0</rpy>
<geom:box name="laser_box">
Modified: pkg/trunk/drivers/robot/pr2/pr2Core/include/pr2Core/pr2Core.h
===================================================================
--- pkg/trunk/drivers/robot/pr2/pr2Core/include/pr2Core/pr2Core.h 2008-07-09 17:13:22 UTC (rev 1361)
+++ pkg/trunk/drivers/robot/pr2/pr2Core/include/pr2Core/pr2Core.h 2008-07-09 17:16:16 UTC (rev 1362)
@@ -6,6 +6,46 @@
namespace PR2
{
+ enum PR2_FRAMEID{
+ FRAMEID_CASTER_FL_WHEEL_L = 101 , // FIXME: this is a hack until we have a frameid server
+ FRAMEID_CASTER_FL_WHEEL_R ,
+ FRAMEID_CASTER_FL_BODY ,
+ FRAMEID_CASTER_FR_WHEEL_L ,
+ FRAMEID_CASTER_FR_WHEEL_R ,
+ FRAMEID_CASTER_FR_BODY ,
+ FRAMEID_CASTER_RL_WHEEL_L ,
+ FRAMEID_CASTER_RL_WHEEL_R ,
+ FRAMEID_CASTER_RL_BODY ,
+ FRAMEID_CASTER_RR_WHEEL_L ,
+ FRAMEID_CASTER_RR_WHEEL_R ,
+ FRAMEID_CASTER_RR_BODY ,
+ FRAMEID_BASE ,
+ FRAMEID_TORSO ,
+ FRAMEID_ARM_L_TURRET ,
+ FRAMEID_ARM_L_SHOULDER ,
+ FRAMEID_ARM_L_UPPERARM ,
+ FRAMEID_ARM_L_ELBOW ,
+ FRAMEID_ARM_L_FOREARM ,
+ FRAMEID_ARM_L_WRIST ,
+ FRAMEID_ARM_L_HAND ,
+ FRAMEID_ARM_L_FINGER_1 ,
+ FRAMEID_ARM_L_FINGER_2 ,
+ FRAMEID_ARM_R_TURRET ,
+ FRAMEID_ARM_R_SHOULDER ,
+ FRAMEID_ARM_R_UPPERARM ,
+ FRAMEID_ARM_R_ELBOW ,
+ FRAMEID_ARM_R_FOREARM ,
+ FRAMEID_ARM_R_WRIST ,
+ FRAMEID_ARM_R_HAND ,
+ FRAMEID_ARM_R_FINGER_1 ,
+ FRAMEID_ARM_R_FINGER_2 ,
+ FRAMEID_HEAD_BASE ,
+ FRAMEID_LASER_BLOCK ,
+ FRAMEID_STEREO_BLOCK ,
+ FRAMEID_LASERBLOCK ,
+ MAX_FRAMEIDS
+ };
+
enum PR2_JOINT_CONTROL_MODE{
TORQUE_CONTROL,
PD_CONTROL,
@@ -100,7 +140,7 @@
ARM_L_ELBOW ,
ARM_L_FOREARM ,
ARM_L_WRIST ,
- ARM_L_GRIPPER_TMP ,
+ ARM_L_HAND ,
ARM_L_FINGER_1 ,
ARM_L_FINGER_2 ,
ARM_R_TURRET ,
@@ -109,7 +149,7 @@
ARM_R_ELBOW ,
ARM_R_FOREARM ,
ARM_R_WRIST ,
- ARM_R_GRIPPER_TMP ,
+ ARM_R_HAND ,
ARM_R_FINGER_1 ,
ARM_R_FINGER_2 ,
HEAD_BASE ,
@@ -243,7 +283,7 @@
const int JointEnd[MAX_MODELS] = {CASTER_RR_DRIVE_R ,SPINE_ELEVATOR ,ARM_L_WRIST_ROLL ,ARM_R_WRIST_ROLL ,ARM_L_GRIPPER ,ARM_R_GRIPPER ,HEAD_LASER_PITCH };
-// Geometric description for the base
+ // Geometric description for the base
const float CASTER_FL_X_OFFSET = 0.25;
const float CASTER_FL_Y_OFFSET = 0.25;
Modified: pkg/trunk/simulators/pr2SimRT/manifest.xml
===================================================================
--- pkg/trunk/simulators/pr2SimRT/manifest.xml 2008-07-09 17:13:22 UTC (rev 1361)
+++ pkg/trunk/simulators/pr2SimRT/manifest.xml 2008-07-09 17:16:16 UTC (rev 1362)
@@ -7,7 +7,7 @@
<author>John M. Hsu</author>
<license>TBD</license>
<depend package="roscpp" />
- <depend package="libpr2GZ" />
+ <depend package="libpr2API" />
<depend package="genericControllers" />
<depend package="pr2Controllers" />
<depend package="pr2Core" />
Modified: pkg/trunk/simulators/rosgazebo/rosgazebo.cc
===================================================================
--- pkg/trunk/simulators/rosgazebo/rosgazebo.cc 2008-07-09 17:13:22 UTC (rev 1361)
+++ pkg/trunk/simulators/rosgazebo/rosgazebo.cc 2008-07-09 17:16:16 UTC (rev 1362)
@@ -505,11 +505,11 @@
this->odomMsg.vel.th = vw;
// Get position
- double x,y,z,roll,pitch,th;
- this->myPR2->GetBasePositionActual(&x,&y,&z,&roll,&pitch,&th);
+ double x,y,z,roll,pitch,yaw;
+ this->myPR2->GetBasePositionActual(&x,&y,&z,&roll,&pitch,&yaw);
this->odomMsg.pos.x = x;
this->odomMsg.pos.y = y;
- this->odomMsg.pos.th = th;
+ this->odomMsg.pos.th = yaw;
// this->odomMsg.stall = this->positionmodel->Stall();
// TODO: get the frame ID from somewhere
@@ -667,7 +667,211 @@
// this->myPR2->OpenGripper(PR2::PR2_LEFT_GRIPPER ,this->arm.gripperGapCmd,this->arm.gripperForceCmd);
// this->myPR2->CloseGripper(PR2::PR2_RIGHT_GRIPPER,this->arm.gripperGapCmd,this->arm.gripperForceCmd);
+ /***************************************************************/
+ /* */
+ /* frame transforms */
+ /* */
+ /* x,y,z,yaw,pitch,roll */
+ /* */
+ /***************************************************************/
+ //double x,y,z,roll,pitch,yaw;
+ //this->myPR2->GetBasePositionActual(&x,&y,&z,&roll,&pitch,&yaw); // actual CoM of base
+ tf.sendInverseEuler(FRAMEID_ROBOT,
+ PR2::FRAMEID_BASE,
+ 0.0,
+ 0.0,
+ -0.13, /* half height of base box */
+ yaw,
+ pitch,
+ roll,
+ odomMsg.header.stamp.sec,
+ odomMsg.header.stamp.nsec);
+ // base = center of the bottom of the base box
+ // torso = midpoint of bottom of turrets
+ tf.sendInverseEuler(PR2::FRAMEID_BASE,
+ PR2::FRAMEID_TORSO,
+ PR2::BASE_LEFT_ARM_OFFSET.x,
+ 0.0,
+ PR2::BASE_LEFT_ARM_OFFSET.z, /* FIXME: spine elevator not accounted for */
+ 0.0,
+ 0.0,
+ 0.0,
+ odomMsg.header.stamp.sec,
+ odomMsg.header.stamp.nsec);
+
+ // arm_l_turret = bottom of left turret
+ tf.sendInverseEuler(PR2::FRAMEID_TORSO,
+ PR2::FRAMEID_ARM_L_TURRET,
+ 0.0,
+ PR2::BASE_LEFT_ARM_OFFSET.y,
+ 0.0,
+ larm.turretAngle,
+ 0.0,
+ 0.0,
+ odomMsg.header.stamp.sec,
+ odomMsg.header.stamp.nsec);
+
+ // arm_l_shoulder = center of left shoulder pitch bracket
+ tf.sendInverseEuler(PR2::FRAMEID_ARM_L_TURRET,
+ PR2::FRAMEID_ARM_L_SHOULDER,
+ PR2::ARM_PAN_SHOULDER_PITCH_OFFSET.x,
+ PR2::ARM_PAN_SHOULDER_PITCH_OFFSET.y,
+ PR2::ARM_PAN_SHOULDER_PITCH_OFFSET.z,
+ 0.0,
+ larm.shoulderLiftAngle,
+ 0.0,
+ odomMsg.header.stamp.sec,
+ odomMsg.header.stamp.nsec);
+
+ // arm_l_upperarm = upper arm with roll DOF, at shoulder pitch center
+ tf.sendInverseEuler(PR2::FRAMEID_ARM_L_SHOULDER,
+ PR2::FRAMEID_ARM_L_UPPERARM,
+ PR2::ARM_SHOULDER_PITCH_ROLL_OFFSET.x,
+ PR2::ARM_SHOULDER_PITCH_ROLL_OFFSET.y,
+ PR2::ARM_SHOULDER_PITCH_ROLL_OFFSET.z,
+ 0.0,
+ 0.0,
+ larm.upperarmRollAngle,
+ odomMsg.header.stamp.sec,
+ odomMsg.header.stamp.nsec);
+
+ //frameid_arm_l_elbow = elbow pitch bracket center of rotation
+ tf.sendInverseEuler(PR2::FRAMEID_ARM_L_UPPERARM,
+ PR2::FRAMEID_ARM_L_ELBOW,
+ PR2::ARM_SHOULDER_ROLL_ELBOW_PITCH_OFFSET.x,
+ PR2::ARM_SHOULDER_ROLL_ELBOW_PITCH_OFFSET.y,
+ PR2::ARM_SHOULDER_ROLL_ELBOW_PITCH_OFFSET.z,
+ 0.0,
+ larm.elbowAngle,
+ 0.0,
+ odomMsg.header.stamp.sec,
+ odomMsg.header.stamp.nsec);
+
+ //frameid_arm_l_forearm = forearm roll DOR, at elbow pitch center
+ tf.sendInverseEuler(PR2::FRAMEID_ARM_L_ELBOW,
+ PR2::FRAMEID_ARM_L_FOREARM,
+ PR2::ELBOW_PITCH_ELBOW_ROLL_OFFSET.x,
+ PR2::ELBOW_PITCH_ELBOW_ROLL_OFFSET.y,
+ PR2::ELBOW_PITCH_ELBOW_ROLL_OFFSET.z,
+ 0.0,
+ 0.0,
+ larm.forearmRollAngle,
+ odomMsg.header.stamp.sec,
+ odomMsg.header.stamp.nsec);
+
+ // arm_l_wrist = wrist pitch DOF.
+ tf.sendInverseEuler(PR2::FRAMEID_ARM_L_FOREARM,
+ PR2::FRAMEID_ARM_L_WRIST,
+ PR2::ELBOW_ROLL_WRIST_PITCH_OFFSET.x,
+ PR2::ELBOW_ROLL_WRIST_PITCH_OFFSET.y,
+ PR2::ELBOW_ROLL_WRIST_PITCH_OFFSET.z,
+ 0.0,
+ larm.wristPitchAngle,
+ 0.0,
+ odomMsg.header.stamp.sec,
+ odomMsg.header.stamp.nsec);
+
+ // arm_l_hand = hand roll DOF, center at wrist pitch center
+ tf.sendInverseEuler(PR2::FRAMEID_ARM_L_WRIST,
+ PR2::FRAMEID_ARM_L_HAND,
+ PR2::WRIST_PITCH_WRIST_ROLL_OFFSET.x,
+ PR2::WRIST_PITCH_WRIST_ROLL_OFFSET.y,
+ PR2::WRIST_PITCH_WRIST_ROLL_OFFSET.z,
+ 0.0,
+ 0.0,
+ larm.wristRollAngle,
+ odomMsg.header.stamp.sec,
+ odomMsg.header.stamp.nsec);
+
+
+ // arm_r_turret = bottom of right turret
+ tf.sendInverseEuler(PR2::FRAMEID_TORSO,
+ PR2::FRAMEID_ARM_R_TURRET,
+ 0.0,
+ PR2::BASE_RIGHT_ARM_OFFSET.y,
+ 0.0,
+ rarm.turretAngle,
+ 0.0,
+ 0.0,
+ odomMsg.header.stamp.sec,
+ odomMsg.header.stamp.nsec);
+
+ // arm_r_shoulder = center of right shoulder pitch bracket
+ tf.sendInverseEuler(PR2::FRAMEID_ARM_R_TURRET,
+ PR2::FRAMEID_ARM_R_SHOULDER,
+ PR2::ARM_PAN_SHOULDER_PITCH_OFFSET.x,
+ PR2::ARM_PAN_SHOULDER_PITCH_OFFSET.y,
+ PR2::ARM_PAN_SHOULDER_PITCH_OFFSET.z,
+ 0.0,
+ rarm.shoulderLiftAngle,
+ 0.0,
+ odomMsg.header.stamp.sec,
+ odomMsg.header.stamp.nsec);
+
+ // arm_r_upperarm = upper arm with roll DOF, at shoulder pitch center
+ tf.sendInverseEuler(PR2::FRAMEID_ARM_R_SHOULDER,
+ PR2::FRAMEID_ARM_R_UPPERARM,
+ PR2::ARM_SHOULDER_PITCH_ROLL_OFFSET.x,
+ PR2::ARM_SHOULDER_PITCH_ROLL_OFFSET.y,
+ PR2::ARM_SHOULDER_PITCH_ROLL_OFFSET.z,
+ 0.0,
+ 0.0,
+ rarm.upperarmRollAngle,
+ odomMsg.header.stamp.sec,
+ odomMsg.header.stamp.nsec);
+
+ //frameid_arm_r_elbow = elbow pitch bracket center of rotation
+ tf.sendInverseEuler(PR2::FRAMEID_ARM_R_UPPERARM,
+ PR2::FRAMEID_ARM_R_ELBOW,
+ PR2::ARM_SHOULDER_ROLL_ELBOW_PITCH_OFFSET.x,
+ PR2::ARM_SHOULDER_ROLL_ELBOW_PITCH_OFFSET.y,
+ PR2::ARM_SHOULDER_ROLL_ELBOW_PITCH_OFFSET.z,
+ 0.0,
+ rarm.elbowAngle,
+ 0.0,
+ odomMsg.header.stamp.sec,
+ odomMsg.header.stamp.nsec);
+
+ //frameid_arm_r_forearm = forearm roll DOR, at elbow pitch center
+ tf.sendInverseEuler(PR2::FRAMEID_ARM_R_ELBOW,
+ PR2::FRAMEID_ARM_R_FOREARM,
+ PR2::ELBOW_PITCH_ELBOW_ROLL_OFFSET.x,
+ PR2::ELBOW_PITCH_ELBOW_ROLL_OFFSET.y,
+ PR2::ELBOW_PITCH_ELBOW_ROLL_OFFSET.z,
+ 0.0,
+ 0.0,
+ rarm.forearmRollAngle,
+ odomMsg.header.stamp.sec,
+ odomMsg.header.stamp.nsec);
+
+ // arm_r_wrist = wrist pitch DOF.
+ tf.sendInverseEuler(PR2::FRAMEID_ARM_R_FOREARM,
+ PR2::FRAMEID_ARM_R_WRIST,
+ PR2::ELBOW_ROLL_WRIST_PITCH_OFFSET.x,
+ PR2::ELBOW_ROLL_WRIST_PITCH_OFFSET.y,
+ PR2::ELBOW_ROLL_WRIST_PITCH_OFFSET.z,
+ 0.0,
+ rarm.wristPitchAngle,
+ 0.0,
+ odomMsg.header.stamp.sec,
+ odomMsg.header.stamp.nsec);
+
+ // arm_r_hand = hand roll DOF, center at wrist pitch center
+ tf.sendInverseEuler(PR2::FRAMEID_ARM_R_WRIST,
+ PR2::FRAMEID_ARM_R_HAND,
+ PR2::WRIST_PITCH_WRIST_ROLL_OFFSET.x,
+ PR2::WRIST_PITCH_WRIST_ROLL_OFFSET.y,
+ PR2::WRIST_PITCH_WRIST_ROLL_OFFSET.z,
+ 0.0,
+ 0.0,
+ rarm.wristRollAngle,
+ odomMsg.header.stamp.sec,
+ odomMsg.header.stamp.nsec);
+
+
+
+
this->lock.unlock();
}
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <hsu...@us...> - 2008-07-09 22:28:12
|
Revision: 1380
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=1380&view=rev
Author: hsujohnhsu
Date: 2008-07-09 15:28:18 -0700 (Wed, 09 Jul 2008)
Log Message:
-----------
updates to pr2 mechanism control code.
update pr2Core AXLE_WIDTH from 0.1 to 0.049 to match function specs.
added guards for rosTF.
Modified Paths:
--------------
pkg/trunk/drivers/robot/pr2/pr2Core/include/pr2Core/pr2Core.h
pkg/trunk/simulators/pr2MechCon/pr2MechCon.cc
pkg/trunk/util/transforms/rosTF/include/rosTF/rosTF.h
Modified: pkg/trunk/drivers/robot/pr2/pr2Core/include/pr2Core/pr2Core.h
===================================================================
--- pkg/trunk/drivers/robot/pr2/pr2Core/include/pr2Core/pr2Core.h 2008-07-09 22:16:31 UTC (rev 1379)
+++ pkg/trunk/drivers/robot/pr2/pr2Core/include/pr2Core/pr2Core.h 2008-07-09 22:28:18 UTC (rev 1380)
@@ -296,7 +296,7 @@
const float CASTER_RR_X_OFFSET = -0.25;
const float CASTER_RR_Y_OFFSET = -0.25;
- const float AXLE_WIDTH = 0.1;
+ const float AXLE_WIDTH = 0.049;
const float WHEEL_RADIUS = 0.08;
const int NUM_CASTERS = 4;
Modified: pkg/trunk/simulators/pr2MechCon/pr2MechCon.cc
===================================================================
--- pkg/trunk/simulators/pr2MechCon/pr2MechCon.cc 2008-07-09 22:16:31 UTC (rev 1379)
+++ pkg/trunk/simulators/pr2MechCon/pr2MechCon.cc 2008-07-09 22:28:18 UTC (rev 1380)
@@ -25,6 +25,12 @@
#include <gazebo/gazebo.h>
#include <gazebo/GazeboError.hh>
#include <libpr2API/pr2API.h>
+#include <pr2Controllers/GripperController.h>
+#include <pr2Controllers/ArmController.h>
+#include <pr2Controllers/HeadController.h>
+#include <pr2Controllers/SpineController.h>
+#include <pr2Controllers/LaserScannerController.h>
+#include <pr2Controllers/BaseController.h>
#include "ringbuffer.h"
// roscpp
@@ -54,7 +60,7 @@
#include <signal.h>
// Our node
-class GazeboNode : public ros::node
+class RosGazeboNode : public ros::node
{
private:
// Messages that we'll send or receive
@@ -81,15 +87,17 @@
// used to generate Gaussian noise (for PCD)
double GaussianKernel(double mu,double sigma);
+ // used to generate Gaussian noise (for PCD)
+ PR2::PR2Robot *PR2Copy;
public:
// Constructor; stage itself needs argc/argv. fname is the .world file
// that stage should load.
- GazeboNode(int argc, char** argv, const char* fname);
- ~GazeboNode();
+ RosGazeboNode(int argc, char** argv, const char* fname,PR2::PR2Robot *myPR2);
+ ~RosGazeboNode();
// advertise / subscribe models
- int SubscribeModels();
+ int AdvertiseSubscribeMessages();
// Do one update of the simulator. May pause if the next update time
// has not yet arrived.
@@ -113,10 +121,6 @@
std_msgs::PR2Arm leftarm;
std_msgs::PR2Arm rightarm;
-
- // The main simulator object
- PR2::PR2Robot* myPR2;
-
// for the point cloud data
ringBuffer<std_msgs::Point3DFloat32> *cloud_pts;
ringBuffer<float> *cloud_ch1;
@@ -129,7 +133,7 @@
};
void
-GazeboNode::cmd_rightarmconfigReceived()
+RosGazeboNode::cmd_rightarmconfigReceived()
{
this->lock.lock();
/*
@@ -152,25 +156,25 @@
this->rightarm.gripperGapCmd};
double jointSpeed[] = {0,0,0,0,0,0,0,0};
-// this->myPR2->SetArmJointPosition(PR2::PR2_LEFT_ARM, jointPosition, jointSpeed);
+ // this->PR2Copy->SetArmJointPosition(PR2::PR2_LEFT_ARM, jointPosition, jointSpeed);
*/
//*
- this->myPR2->hw.SetJointServoCmd(PR2::ARM_R_PAN , this->rightarm.turretAngle, 0);
- this->myPR2->hw.SetJointServoCmd(PR2::ARM_R_SHOULDER_PITCH, this->rightarm.shoulderLiftAngle, 0);
- this->myPR2->hw.SetJointServoCmd(PR2::ARM_R_SHOULDER_ROLL , this->rightarm.upperarmRollAngle, 0);
- this->myPR2->hw.SetJointServoCmd(PR2::ARM_R_ELBOW_PITCH , this->rightarm.elbowAngle, 0);
- this->myPR2->hw.SetJointServoCmd(PR2::ARM_R_ELBOW_ROLL , this->rightarm.forearmRollAngle, 0);
- this->myPR2->hw.SetJointServoCmd(PR2::ARM_R_WRIST_PITCH , this->rightarm.wristPitchAngle, 0);
- this->myPR2->hw.SetJointServoCmd(PR2::ARM_R_WRIST_ROLL , this->rightarm.wristRollAngle, 0);
- this->myPR2->hw.SetJointServoCmd(PR2::ARM_R_GRIPPER , this->rightarm.gripperGapCmd, 0);
- this->myPR2->hw.CloseGripper(PR2::PR2_RIGHT_GRIPPER, this->rightarm.gripperGapCmd, this->rightarm.gripperForceCmd);
+ this->PR2Copy->hw.SetJointServoCmd(PR2::ARM_R_PAN , this->rightarm.turretAngle, 0);
+ this->PR2Copy->hw.SetJointServoCmd(PR2::ARM_R_SHOULDER_PITCH, this->rightarm.shoulderLiftAngle, 0);
+ this->PR2Copy->hw.SetJointServoCmd(PR2::ARM_R_SHOULDER_ROLL , this->rightarm.upperarmRollAngle, 0);
+ this->PR2Copy->hw.SetJointServoCmd(PR2::ARM_R_ELBOW_PITCH , this->rightarm.elbowAngle, 0);
+ this->PR2Copy->hw.SetJointServoCmd(PR2::ARM_R_ELBOW_ROLL , this->rightarm.forearmRollAngle, 0);
+ this->PR2Copy->hw.SetJointServoCmd(PR2::ARM_R_WRIST_PITCH , this->rightarm.wristPitchAngle, 0);
+ this->PR2Copy->hw.SetJointServoCmd(PR2::ARM_R_WRIST_ROLL , this->rightarm.wristRollAngle, 0);
+ this->PR2Copy->hw.SetJointServoCmd(PR2::ARM_R_GRIPPER , this->rightarm.gripperGapCmd, 0);
+ this->PR2Copy->hw.CloseGripper(PR2::PR2_RIGHT_GRIPPER, this->rightarm.gripperGapCmd, this->rightarm.gripperForceCmd);
//*/
this->lock.unlock();
}
void
-GazeboNode::cmd_leftarmconfigReceived()
+RosGazeboNode::cmd_leftarmconfigReceived()
{
this->lock.lock();
/*
@@ -183,25 +187,25 @@
this->leftarm.wristRollAngle,
this->leftarm.gripperGapCmd};
double jointSpeed[] = {0,0,0,0,0,0,0,0};
- this->myPR2->SetArmJointPosition(PR2::PR2_LEFT_ARM, jointPosition, jointSpeed);
+ this->PR2Copy->SetArmJointPosition(PR2::PR2_LEFT_ARM, jointPosition, jointSpeed);
*/
//*
- this->myPR2->hw.SetJointServoCmd(PR2::ARM_L_PAN , this->leftarm.turretAngle, 0);
- this->myPR2->hw.SetJointServoCmd(PR2::ARM_L_SHOULDER_PITCH, this->leftarm.shoulderLiftAngle, 0);
- this->myPR2->hw.SetJointServoCmd(PR2::ARM_L_SHOULDER_ROLL , this->leftarm.upperarmRollAngle, 0);
- this->myPR2->hw.SetJointServoCmd(PR2::ARM_L_ELBOW_PITCH , this->leftarm.elbowAngle, 0);
- this->myPR2->hw.SetJointServoCmd(PR2::ARM_L_ELBOW_ROLL , this->leftarm.forearmRollAngle, 0);
- this->myPR2->hw.SetJointServoCmd(PR2::ARM_L_WRIST_PITCH , this->leftarm.wristPitchAngle, 0);
- this->myPR2->hw.SetJointServoCmd(PR2::ARM_L_WRIST_ROLL , this->leftarm.wristRollAngle, 0);
-// this->myPR2->SetJointServoCmd(PR2::ARM_L_GRIPPER , this->leftarm.gripperGapCmd, 0);
- this->myPR2->hw.CloseGripper(PR2::PR2_LEFT_GRIPPER, this->leftarm.gripperGapCmd, this->leftarm.gripperForceCmd);
+ this->PR2Copy->hw.SetJointServoCmd(PR2::ARM_L_PAN , this->leftarm.turretAngle, 0);
+ this->PR2Copy->hw.SetJointServoCmd(PR2::ARM_L_SHOULDER_PITCH, this->leftarm.shoulderLiftAngle, 0);
+ this->PR2Copy->hw.SetJointServoCmd(PR2::ARM_L_SHOULDER_ROLL , this->leftarm.upperarmRollAngle, 0);
+ this->PR2Copy->hw.SetJointServoCmd(PR2::ARM_L_ELBOW_PITCH , this->leftarm.elbowAngle, 0);
+ this->PR2Copy->hw.SetJointServoCmd(PR2::ARM_L_ELBOW_ROLL , this->leftarm.forearmRollAngle, 0);
+ this->PR2Copy->hw.SetJointServoCmd(PR2::ARM_L_WRIST_PITCH , this->leftarm.wristPitchAngle, 0);
+ this->PR2Copy->hw.SetJointServoCmd(PR2::ARM_L_WRIST_ROLL , this->leftarm.wristRollAngle, 0);
+ // this->PR2Copy->SetJointServoCmd(PR2::ARM_L_GRIPPER , this->leftarm.gripperGapCmd, 0);
+ this->PR2Copy->hw.CloseGripper(PR2::PR2_LEFT_GRIPPER, this->leftarm.gripperGapCmd, this->leftarm.gripperForceCmd);
//*/
this->lock.unlock();
}
void
-GazeboNode::cmdvelReceived()
+RosGazeboNode::cmdvelReceived()
{
this->lock.lock();
double dt;
@@ -209,7 +213,7 @@
// smooth out the commands by time decay
// with w1,w2=1, this means equal weighting for new command every second
- this->myPR2->GetTime(&(this->simTime));
+ this->PR2Copy->GetTime(&(this->simTime));
dt = simTime - lastTime;
// smooth if dt is larger than zero
@@ -228,19 +232,19 @@
// std::cout << "received cmd: vx " << this->velMsg.vx << " vw " << this->velMsg.vw
// << " vxsmoo " << this->vxSmooth << " vxsmoo " << this->vwSmooth
- // << " | steer erros: " << this->myPR2->BaseSteeringAngleError() << " - " << M_PI/100.0
+ // << " | steer erros: " << this->PR2Copy->BaseSteeringAngleError() << " - " << M_PI/100.0
// << std::endl;
// 2dnav: if steering angle is wrong, don't move or move slowly
- if (this->myPR2->BaseSteeringAngleError() > M_PI/100.0)
+ if (this->PR2Copy->BaseSteeringAngleError() > M_PI/100.0)
{
// set steering angle only
- this->myPR2->SetBaseSteeringAngle (this->vxSmooth,0.0,this->vwSmooth);
+ this->PR2Copy->SetBaseSteeringAngle (this->vxSmooth,0.0,this->vwSmooth);
}
else
{
// set base velocity
- this->myPR2->SetBaseCartesianSpeedCmd(this->vxSmooth, 0.0, this->vwSmooth);
+ this->PR2Copy->SetBaseCartesianSpeedCmd(this->vxSmooth, 0.0, this->vwSmooth);
}
// TODO: this is a hack, need to rewrite
@@ -248,7 +252,7 @@
if (this->velMsg.vx == 0.0)
{
// set base velocity
- this->myPR2->SetBaseCartesianSpeedCmd(this->vxSmooth, 0.0, this->vwSmooth);
+ this->PR2Copy->SetBaseCartesianSpeedCmd(this->vxSmooth, 0.0, this->vwSmooth);
}
this->lastTime = this->simTime;
@@ -256,24 +260,15 @@
this->lock.unlock();
}
-GazeboNode::GazeboNode(int argc, char** argv, const char* fname) :
- ros::node("pr2SimRT"),tf(*this)
+RosGazeboNode::RosGazeboNode(int argc, char** argv, const char* fname, PR2::PR2Robot *myPR2) :
+ ros::node("rosgazebo"),tf(*this)
{
+ // accept passed in robot
+ this->PR2Copy = myPR2;
+
// initialize random seed
srand(time(NULL));
- // Initialize robot object
- this->myPR2 = new PR2::PR2Robot();
- // Initialize connections
- this->myPR2->InitializeRobot();
- // Set control mode for the base
- this->myPR2->SetBaseControlMode(PR2::PR2_CARTESIAN_CONTROL);
- // this->myPR2->SetJointControlMode(PR2::CASTER_FL_STEER, PR2::TORQUE_CONTROL);
- // this->myPR2->SetJointControlMode(PR2::CASTER_FR_STEER, PR2::TORQUE_CONTROL);
- // this->myPR2->SetJointControlMode(PR2::CASTER_RL_STEER, PR2::TORQUE_CONTROL);
- // this->myPR2->SetJointControlMode(PR2::CASTER_RR_STEER, PR2::TORQUE_CONTROL);
-
-
// Initialize ring buffer for point cloud data
this->cloud_pts = new ringBuffer<std_msgs::Point3DFloat32>();
this->cloud_ch1 = new ringBuffer<float>();
@@ -283,30 +278,13 @@
this->cloud_pts->allocate(this->max_cloud_pts);
this->cloud_ch1->allocate(this->max_cloud_pts);
- // Set control mode for the arms
- // FIXME: right now this just sets default to pd control
- //this->myPR2->SetArmControlMode(PR2::PR2_RIGHT_ARM, PR2::PR2_JOINT_CONTROL);
- //this->myPR2->SetArmControlMode(PR2::PR2_LEFT_ARM, PR2::PR2_JOINT_CONTROL);
- //------------------------------------------------------------
+ // initialize times
+ this->PR2Copy->GetTime(&(this->lastTime));
+ this->PR2Copy->GetTime(&(this->simTime));
- this->myPR2->EnableGripperLeft();
- this->myPR2->EnableGripperRight();
-
- this->myPR2->GetTime(&(this->lastTime));
- this->myPR2->GetTime(&(this->simTime));
-
- // set torques for driving the robot wheels
- // this->myPR2->hw.SetJointTorque(PR2::CASTER_FL_DRIVE_L, 1000.0 );
- // this->myPR2->hw.SetJointTorque(PR2::CASTER_FR_DRIVE_L, 1000.0 );
- // this->myPR2->hw.SetJointTorque(PR2::CASTER_RL_DRIVE_L, 1000.0 );
- // this->myPR2->hw.SetJointTorque(PR2::CASTER_RR_DRIVE_L, 1000.0 );
- // this->myPR2->hw.SetJointTorque(PR2::CASTER_FL_DRIVE_R, 1000.0 );
- // this->myPR2->hw.SetJointTorque(PR2::CASTER_FR_DRIVE_R, 1000.0 );
- // this->myPR2->hw.SetJointTorque(PR2::CASTER_RL_DRIVE_R, 1000.0 );
- // this->myPR2->hw.SetJointTorque(PR2::CASTER_RR_DRIVE_R, 1000.0 );
}
-void GazeboNode::finalize(int)
+void RosGazeboNode::finalize(int)
{
fprintf(stderr,"Caught sig, clean-up and exit\n");
sleep(1);
@@ -315,7 +293,7 @@
int
-GazeboNode::SubscribeModels()
+RosGazeboNode::AdvertiseSubscribeMessages()
{
//advertise<std_msgs::LaserScan>("laser");
advertise<std_msgs::LaserScan>("scan");
@@ -328,20 +306,19 @@
advertise<std_msgs::PR2Arm>("right_pr2arm_pos");
advertise<rostools::Time>("time");
- subscribe("cmd_vel", velMsg, &GazeboNode::cmdvelReceived);
- subscribe("cmd_leftarmconfig", leftarm, &GazeboNode::cmd_leftarmconfigReceived);
- subscribe("cmd_rightarmconfig", rightarm, &GazeboNode::cmd_rightarmconfigReceived);
+ subscribe("cmd_vel", velMsg, &RosGazeboNode::cmdvelReceived);
+ subscribe("cmd_leftarmconfig", leftarm, &RosGazeboNode::cmd_leftarmconfigReceived);
+ subscribe("cmd_rightarmconfig", rightarm, &RosGazeboNode::cmd_rightarmconfigReceived);
-
return(0);
}
-GazeboNode::~GazeboNode()
+RosGazeboNode::~RosGazeboNode()
{
}
double
-GazeboNode::GaussianKernel(double mu,double sigma)
+RosGazeboNode::GaussianKernel(double mu,double sigma)
{
// using Box-Muller transform to generate two independent standard normally disbributed normal variables
// see wikipedia
@@ -356,7 +333,7 @@
}
void
-GazeboNode::Update()
+RosGazeboNode::Update()
{
this->lock.lock();
@@ -375,7 +352,7 @@
/* laser - pitching */
/* */
/***************************************************************/
- if (this->myPR2->hw.GetLaserRanges(PR2::LASER_HEAD,
+ if (this->PR2Copy->hw.GetLaserRanges(PR2::LASER_HEAD,
&angle_min, &angle_max, &angle_increment,
&range_max, &ranges_size , &ranges_alloc_size,
&intensities_size, &intensities_alloc_size,
@@ -385,7 +362,7 @@
{
// get laser pitch angle
double laser_yaw, laser_pitch, laser_pitch_rate;
- this->myPR2->hw.GetJointServoActual(PR2::HEAD_LASER_PITCH , &laser_pitch, &laser_pitch_rate);
+ this->PR2Copy->hw.GetJointServoActual(PR2::HEAD_LASER_PITCH , &laser_pitch, &laser_pitch_rate);
// get laser yaw angle
laser_yaw = angle_min + (double)i * angle_increment;
//std::cout << " pit " << laser_pitch << "yaw " << laser_yaw
@@ -450,7 +427,7 @@
/* publish time */
/* */
/***************************************************************/
- this->myPR2->GetTime(&(this->simTime));
+ this->PR2Copy->GetTime(&(this->simTime));
timeMsg.rostime.sec = (unsigned long)floor(this->simTime);
timeMsg.rostime.nsec = (unsigned long)floor( 1e9 * ( this->simTime - this->laserMsg.header.stamp.sec) );
publish("time",timeMsg);
@@ -460,7 +437,7 @@
/* laser - base */
/* */
/***************************************************************/
- if (this->myPR2->hw.GetLaserRanges(PR2::LASER_BASE,
+ if (this->PR2Copy->hw.GetLaserRanges(PR2::LASER_BASE,
&angle_min, &angle_max, &angle_increment,
&range_max, &ranges_size , &ranges_alloc_size,
&intensities_size, &intensities_alloc_size,
@@ -498,18 +475,18 @@
// Get latest odometry data
// Get velocities
double vx,vy,vw;
- this->myPR2->GetBaseCartesianSpeedActual(&vx,&vy,&vw);
+ this->PR2Copy->GetBaseCartesianSpeedActual(&vx,&vy,&vw);
// Translate into ROS message format and publish
this->odomMsg.vel.x = vx;
this->odomMsg.vel.y = vy;
this->odomMsg.vel.th = vw;
// Get position
- double x,y,z,roll,pitch,th;
- this->myPR2->GetBasePositionActual(&x,&y,&z,&roll,&pitch,&th);
+ double x,y,z,roll,pitch,yaw;
+ this->PR2Copy->GetBasePositionActual(&x,&y,&z,&roll,&pitch,&yaw);
this->odomMsg.pos.x = x;
this->odomMsg.pos.y = y;
- this->odomMsg.pos.th = th;
+ this->odomMsg.pos.th = yaw;
// this->odomMsg.stall = this->positionmodel->Stall();
// TODO: get the frame ID from somewhere
@@ -550,8 +527,8 @@
static unsigned char buf[GAZEBO_CAMERA_MAX_IMAGE_SIZE];
// get image
- //this->myPR2->hw.GetCameraImage(PR2::CAMERA_GLOBAL,
- this->myPR2->hw.GetCameraImage(PR2::CAMERA_HEAD_RIGHT,
+ //this->PR2Copy->hw.GetCameraImage(PR2::CAMERA_GLOBAL,
+ this->PR2Copy->hw.GetCameraImage(PR2::CAMERA_HEAD_RIGHT,
&width , &height ,
&depth ,
&compression , &colorspace ,
@@ -584,12 +561,12 @@
simPitchOffset = -M_PI / 8.0;
simPitchAngle = simPitchOffset + simPitchAmp * sin(this->simTime * simPitchTimeScale);
simPitchRate = simPitchAmp * simPitchTimeScale * cos(this->simTime * simPitchTimeScale); // TODO: check rate correctness
- this->myPR2->GetTime(&this->simTime);
+ this->PR2Copy->GetTime(&this->simTime);
//std::cout << "sim time: " << this->simTime << std::endl;
//std::cout << "ang: " << simPitchAngle*180.0/M_PI << "rate: " << simPitchRate*180.0/M_PI << std::endl;
- this->myPR2->hw.SetJointTorque(PR2::HEAD_LASER_PITCH , 1000.0);
- this->myPR2->hw.SetJointGains(PR2::HEAD_LASER_PITCH, 10.0, 0.0, 0.0);
- this->myPR2->hw.SetJointServoCmd(PR2::HEAD_LASER_PITCH , simPitchAngle, simPitchRate);
+ this->PR2Copy->hw.SetJointTorque(PR2::HEAD_LASER_PITCH , 1000.0);
+ this->PR2Copy->hw.SetJointGains(PR2::HEAD_LASER_PITCH, 10.0, 0.0, 0.0);
+ this->PR2Copy->hw.SetJointServoCmd(PR2::HEAD_LASER_PITCH , simPitchAngle, simPitchRate);
if (dAngle * simPitchRate < 0.0)
{
@@ -611,63 +588,495 @@
std_msgs::PR2Arm larm, rarm;
/* get left arm position */
- this->myPR2->hw.GetJointPositionActual(PR2::ARM_L_PAN, &position, &velocity);
+ this->PR2Copy->hw.GetJointPositionActual(PR2::ARM_L_PAN, &position, &velocity);
larm.turretAngle = position;
- this->myPR2->hw.GetJointPositionActual(PR2::ARM_L_SHOULDER_PITCH, &position, &velocity);
+ this->PR2Copy->hw.GetJointPositionActual(PR2::ARM_L_SHOULDER_PITCH, &position, &velocity);
larm.shoulderLiftAngle = position;
- this->myPR2->hw.GetJointPositionActual(PR2::ARM_L_SHOULDER_ROLL, &position, &velocity);
+ this->PR2Copy->hw.GetJointPositionActual(PR2::ARM_L_SHOULDER_ROLL, &position, &velocity);
larm.upperarmRollAngle = position;
- this->myPR2->hw.GetJointPositionActual(PR2::ARM_L_ELBOW_PITCH, &position, &velocity);
+ this->PR2Copy->hw.GetJointPositionActual(PR2::ARM_L_ELBOW_PITCH, &position, &velocity);
larm.elbowAngle = position;
- this->myPR2->hw.GetJointPositionActual(PR2::ARM_L_ELBOW_ROLL, &position, &velocity);
+ this->PR2Copy->hw.GetJointPositionActual(PR2::ARM_L_ELBOW_ROLL, &position, &velocity);
larm.forearmRollAngle = position;
- this->myPR2->hw.GetJointPositionActual(PR2::ARM_L_WRIST_PITCH, &position, &velocity);
+ this->PR2Copy->hw.GetJointPositionActual(PR2::ARM_L_WRIST_PITCH, &position, &velocity);
larm.wristPitchAngle = position;
- this->myPR2->hw.GetJointPositionActual(PR2::ARM_L_WRIST_ROLL, &position, &velocity);
+ this->PR2Copy->hw.GetJointPositionActual(PR2::ARM_L_WRIST_ROLL, &position, &velocity);
larm.wristRollAngle = position;
- this->myPR2->GetLeftGripperActual ( &position, &velocity);
+ this->PR2Copy->GetLeftGripperActual ( &position, &velocity);
larm.gripperForceCmd = velocity;
larm.gripperGapCmd = position;
publish("left_pr2arm_pos", larm);
/* get left arm position */
- this->myPR2->hw.GetJointPositionActual(PR2::ARM_R_PAN, &position, &velocity);
+ this->PR2Copy->hw.GetJointPositionActual(PR2::ARM_R_PAN, &position, &velocity);
rarm.turretAngle = position;
- this->myPR2->hw.GetJointPositionActual(PR2::ARM_R_SHOULDER_PITCH, &position, &velocity);
+ this->PR2Copy->hw.GetJointPositionActual(PR2::ARM_R_SHOULDER_PITCH, &position, &velocity);
rarm.shoulderLiftAngle = position;
- this->myPR2->hw.GetJointPositionActual(PR2::ARM_R_SHOULDER_ROLL, &position, &velocity);
+ this->PR2Copy->hw.GetJointPositionActual(PR2::ARM_R_SHOULDER_ROLL, &position, &velocity);
rarm.upperarmRollAngle = position;
- this->myPR2->hw.GetJointPositionActual(PR2::ARM_R_ELBOW_PITCH, &position, &velocity);
+ this->PR2Copy->hw.GetJointPositionActual(PR2::ARM_R_ELBOW_PITCH, &position, &velocity);
rarm.elbowAngle = position;
- this->myPR2->hw.GetJointPositionActual(PR2::ARM_R_ELBOW_ROLL, &position, &velocity);
+ this->PR2Copy->hw.GetJointPositionActual(PR2::ARM_R_ELBOW_ROLL, &position, &velocity);
rarm.forearmRollAngle = position;
- this->myPR2->hw.GetJointPositionActual(PR2::ARM_R_WRIST_PITCH, &position, &velocity);
+ this->PR2Copy->hw.GetJointPositionActual(PR2::ARM_R_WRIST_PITCH, &position, &velocity);
rarm.wristPitchAngle = position;
- this->myPR2->hw.GetJointPositionActual(PR2::ARM_R_WRIST_ROLL, &position, &velocity);
+ this->PR2Copy->hw.GetJointPositionActual(PR2::ARM_R_WRIST_ROLL, &position, &velocity);
rarm.wristRollAngle = position;
- this->myPR2->GetRightGripperActual ( &position, &velocity);
+ this->PR2Copy->GetRightGripperActual ( &position, &velocity);
rarm.gripperForceCmd = velocity;
rarm.gripperGapCmd = position;
publish("right_pr2arm_pos", rarm);
-// this->arm.turretAngle = 0.0;
-// this->arm.shoulderLiftAngle = 0.0;
-// this->arm.upperarmRollAngle = 0.0;
-// this->arm.elbowAngle = 0.0;
-// this->arm.forearmRollAngle = 0.0;
-// this->arm.wristPitchAngle = 0.0;
-// this->arm.wristRollAngle = 0.0;
-// this->arm.gripperForceCmd = 1000.0;
-// this->arm.gripperGapCmd = 0.0;
-//
-// // gripper test
-// this->myPR2->SetGripperGains(PR2::PR2_LEFT_GRIPPER ,10.0,0.0,0.0);
-// this->myPR2->SetGripperGains(PR2::PR2_RIGHT_GRIPPER ,10.0,0.0,0.0);
-// this->myPR2->OpenGripper(PR2::PR2_LEFT_GRIPPER ,this->arm.gripperGapCmd,this->arm.gripperForceCmd);
-// this->myPR2->CloseGripper(PR2::PR2_RIGHT_GRIPPER,this->arm.gripperGapCmd,this->arm.gripperForceCmd);
+ // this->arm.turretAngle = 0.0;
+ // this->arm.shoulderLiftAngle = 0.0;
+ // this->arm.upperarmRollAngle = 0.0;
+ // this->arm.elbowAngle = 0.0;
+ // this->arm.forearmRollAngle = 0.0;
+ // this->arm.wristPitchAngle = 0.0;
+ // this->arm.wristRollAngle = 0.0;
+ // this->arm.gripperForceCmd = 1000.0;
+ // this->arm.gripperGapCmd = 0.0;
+ //
+ // // gripper test
+ // this->PR2Copy->SetGripperGains(PR2::PR2_LEFT_GRIPPER ,10.0,0.0,0.0);
+ // this->PR2Copy->SetGripperGains(PR2::PR2_RIGHT_GRIPPER ,10.0,0.0,0.0);
+ // this->PR2Copy->OpenGripper(PR2::PR2_LEFT_GRIPPER ,this->arm.gripperGapCmd,this->arm.gripperForceCmd);
+ // this->PR2Copy->CloseGripper(PR2::PR2_RIGHT_GRIPPER,this->arm.gripperGapCmd,this->arm.gripperForceCmd);
+ /***************************************************************/
+ /* */
+ /* frame transforms */
+ /* */
+ /* x,y,z,yaw,pitch,roll */
+ /* */
+ /***************************************************************/
+ //this->PR2Copy->GetBasePositionActual(&x,&y,&z,&roll,&pitch,&yaw); // actual CoM of base
+ tf.sendInverseEuler(PR2::FRAMEID_BASE,
+ FRAMEID_ODOM,
+ x,
+ y,
+ z-0.13, /* half height of base box */
+ yaw,
+ pitch,
+ roll,
+ odomMsg.header.stamp.sec,
+ odomMsg.header.stamp.nsec);
+ // base = center of the bottom of the base box
+ // torso = midpoint of bottom of turrets
+ tf.sendInverseEuler(PR2::FRAMEID_TORSO,
+ PR2::FRAMEID_BASE,
+ PR2::BASE_LEFT_ARM_OFFSET.x,
+ 0.0,
+ PR2::BASE_LEFT_ARM_OFFSET.z, /* FIXME: spine elevator not accounted for */
+ 0.0,
+ 0.0,
+ 0.0,
+ odomMsg.header.stamp.sec,
+ odomMsg.header.stamp.nsec);
+
+ // arm_l_turret = bottom of left turret
+ tf.sendInverseEuler(PR2::FRAMEID_ARM_L_TURRET,
+ PR2::FRAMEID_TORSO,
+ 0.0,
+ PR2::BASE_LEFT_ARM_OFFSET.y,
+ 0.0,
+ larm.turretAngle,
+ 0.0,
+ 0.0,
+ odomMsg.header.stamp.sec,
+ odomMsg.header.stamp.nsec);
+
+ // arm_l_shoulder = center of left shoulder pitch bracket
+ tf.sendInverseEuler(PR2::FRAMEID_ARM_L_SHOULDER,
+ PR2::FRAMEID_ARM_L_TURRET,
+ PR2::ARM_PAN_SHOULDER_PITCH_OFFSET.x,
+ PR2::ARM_PAN_SHOULDER_PITCH_OFFSET.y,
+ PR2::ARM_PAN_SHOULDER_PITCH_OFFSET.z,
+ 0.0,
+ larm.shoulderLiftAngle,
+ 0.0,
+ odomMsg.header.stamp.sec,
+ odomMsg.header.stamp.nsec);
+
+ // arm_l_upperarm = upper arm with roll DOF, at shoulder pitch center
+ tf.sendInverseEuler(PR2::FRAMEID_ARM_L_UPPERARM,
+ PR2::FRAMEID_ARM_L_SHOULDER,
+ PR2::ARM_SHOULDER_PITCH_ROLL_OFFSET.x,
+ PR2::ARM_SHOULDER_PITCH_ROLL_OFFSET.y,
+ PR2::ARM_SHOULDER_PITCH_ROLL_OFFSET.z,
+ 0.0,
+ 0.0,
+ larm.upperarmRollAngle,
+ odomMsg.header.stamp.sec,
+ odomMsg.header.stamp.nsec);
+
+ //frameid_arm_l_elbow = elbow pitch bracket center of rotation
+ tf.sendInverseEuler(PR2::FRAMEID_ARM_L_ELBOW,
+ PR2::FRAMEID_ARM_L_UPPERARM,
+ PR2::ARM_SHOULDER_ROLL_ELBOW_PITCH_OFFSET.x,
+ PR2::ARM_SHOULDER_ROLL_ELBOW_PITCH_OFFSET.y,
+ PR2::ARM_SHOULDER_ROLL_ELBOW_PITCH_OFFSET.z,
+ 0.0,
+ larm.elbowAngle,
+ 0.0,
+ odomMsg.header.stamp.sec,
+ odomMsg.header.stamp.nsec);
+
+ //frameid_arm_l_forearm = forearm roll DOR, at elbow pitch center
+ tf.sendInverseEuler(PR2::FRAMEID_ARM_L_FOREARM,
+ PR2::FRAMEID_ARM_L_ELBOW,
+ PR2::ELBOW_PITCH_ELBOW_ROLL_OFFSET.x,
+ PR2::ELBOW_PITCH_ELBOW_ROLL_OFFSET.y,
+ PR2::ELBOW_PITCH_ELBOW_ROLL_OFFSET.z,
+ 0.0,
+ 0.0,
+ larm.forearmRollAngle,
+ odomMsg.header.stamp.sec,
+ odomMsg.header.stamp.nsec);
+
+ // arm_l_wrist = wrist pitch DOF.
+ tf.sendInverseEuler(PR2::FRAMEID_ARM_L_WRIST,
+ PR2::FRAMEID_ARM_L_FOREARM,
+ PR2::ELBOW_ROLL_WRIST_PITCH_OFFSET.x,
+ PR2::ELBOW_ROLL_WRIST_PITCH_OFFSET.y,
+ PR2::ELBOW_ROLL_WRIST_PITCH_OFFSET.z,
+ 0.0,
+ larm.wristPitchAngle,
+ 0.0,
+ odomMsg.header.stamp.sec,
+ odomMsg.header.stamp.nsec);
+
+ // arm_l_hand = hand roll DOF, center at wrist pitch center
+ tf.sendInverseEuler(PR2::FRAMEID_ARM_L_HAND,
+ PR2::FRAMEID_ARM_L_WRIST,
+ PR2::WRIST_PITCH_WRIST_ROLL_OFFSET.x,
+ PR2::WRIST_PITCH_WRIST_ROLL_OFFSET.y,
+ PR2::WRIST_PITCH_WRIST_ROLL_OFFSET.z,
+ 0.0,
+ 0.0,
+ larm.wristRollAngle,
+ odomMsg.header.stamp.sec,
+ odomMsg.header.stamp.nsec);
+
+ // FIXME: not implemented
+ tf.sendInverseEuler(PR2::FRAMEID_ARM_L_FINGER_1,
+ PR2::FRAMEID_ARM_L_HAND,
+ 0.05,
+ 0.025,
+ 0.0,
+ 0.0,
+ 0.0,
+ 0.0,
+ odomMsg.header.stamp.sec,
+ odomMsg.header.stamp.nsec);
+
+ // FIXME: not implemented
+ tf.sendInverseEuler(PR2::FRAMEID_ARM_L_FINGER_2,
+ PR2::FRAMEID_ARM_L_HAND,
+ 0.05,
+ -0.025,
+ 0.0,
+ 0.0,
+ 0.0,
+ 0.0,
+ odomMsg.header.stamp.sec,
+ odomMsg.header.stamp.nsec);
+
+
+ // arm_r_turret = bottom of right turret
+ tf.sendInverseEuler(PR2::FRAMEID_ARM_R_TURRET,
+ PR2::FRAMEID_TORSO,
+ 0.0,
+ PR2::BASE_RIGHT_ARM_OFFSET.y,
+ 0.0,
+ rarm.turretAngle,
+ 0.0,
+ 0.0,
+ odomMsg.header.stamp.sec,
+ odomMsg.header.stamp.nsec);
+
+ // arm_r_shoulder = center of right shoulder pitch bracket
+ tf.sendInverseEuler(PR2::FRAMEID_ARM_R_SHOULDER,
+ PR2::FRAMEID_ARM_R_TURRET,
+ PR2::ARM_PAN_SHOULDER_PITCH_OFFSET.x,
+ PR2::ARM_PAN_SHOULDER_PITCH_OFFSET.y,
+ PR2::ARM_PAN_SHOULDER_PITCH_OFFSET.z,
+ 0.0,
+ rarm.shoulderLiftAngle,
+ 0.0,
+ odomMsg.header.stamp.sec,
+ odomMsg.header.stamp.nsec);
+
+ // arm_r_upperarm = upper arm with roll DOF, at ...
[truncated message content] |
|
From: <hsu...@us...> - 2008-07-10 00:38:26
|
Revision: 1388
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=1388&view=rev
Author: hsujohnhsu
Date: 2008-07-09 17:38:34 -0700 (Wed, 09 Jul 2008)
Log Message:
-----------
updates to mechanism control loop and controllers.
Modified Paths:
--------------
pkg/trunk/controllers/genericControllers/src/Controller.cpp
pkg/trunk/controllers/pr2Controllers/include/pr2Controllers/ArmController.h
pkg/trunk/controllers/pr2Controllers/include/pr2Controllers/BaseController.h
pkg/trunk/controllers/pr2Controllers/include/pr2Controllers/GripperController.h
pkg/trunk/controllers/pr2Controllers/include/pr2Controllers/HeadController.h
pkg/trunk/controllers/pr2Controllers/include/pr2Controllers/LaserScannerController.h
pkg/trunk/controllers/pr2Controllers/include/pr2Controllers/SpineController.h
pkg/trunk/controllers/pr2Controllers/src/ArmController.cpp
pkg/trunk/controllers/pr2Controllers/src/BaseController.cpp
pkg/trunk/controllers/pr2Controllers/src/GripperController.cpp
pkg/trunk/controllers/pr2Controllers/src/HeadController.cpp
pkg/trunk/controllers/pr2Controllers/src/LaserScannerController.cpp
pkg/trunk/controllers/pr2Controllers/src/SpineController.cpp
pkg/trunk/simulators/pr2MechCon/pr2MechCon.cc
Modified: pkg/trunk/controllers/genericControllers/src/Controller.cpp
===================================================================
--- pkg/trunk/controllers/genericControllers/src/Controller.cpp 2008-07-10 00:35:28 UTC (rev 1387)
+++ pkg/trunk/controllers/genericControllers/src/Controller.cpp 2008-07-10 00:38:34 UTC (rev 1388)
@@ -20,6 +20,7 @@
Controller::Controller()
{
}
+
Controller::~Controller()
{
}
Modified: pkg/trunk/controllers/pr2Controllers/include/pr2Controllers/ArmController.h
===================================================================
--- pkg/trunk/controllers/pr2Controllers/include/pr2Controllers/ArmController.h 2008-07-10 00:35:28 UTC (rev 1387)
+++ pkg/trunk/controllers/pr2Controllers/include/pr2Controllers/ArmController.h 2008-07-10 00:38:34 UTC (rev 1388)
@@ -20,119 +20,125 @@
#include <libpr2HW/pr2HW.h>
#include <genericControllers/Controller.h>
-
-
-class ArmController : CONTROLLER::Controller
+namespace CONTROLLER
{
- public:
-
- /*!
- * \brief Constructor.
- *
- * \param
- */
- ArmController();
+ class ArmController : Controller
+ {
+ public:
- /*!
- * \brief Destructor.
- */
- ~ArmController( );
+ /*!
+ * \brief Constructor.
+ *
+ * \param
+ */
+ ArmController();
+
+ /*!
+ * \brief Destructor.
+ */
+ ~ArmController( );
- /*!
- *
- * \brief Set Cartesian position of Hand (end-effector) in Global Frame (Euler Angles)
- *
- */
- PR2::PR2_ERROR_CODE setHandCartesianPosition (double x, double y, double z, double roll, double pitch, double yaw);
+ /*!
+ * \brief Update controller
+ */
+ void Update( );
- PR2::PR2_ERROR_CODE getHandCartesianPositionCmd(double *x, double *y, double *z, double *roll, double *pitch, double *yaw);
- PR2::PR2_ERROR_CODE getHandCartesianPositionAct(double *x, double *y, double *z, double *roll, double *pitch, double *yaw);
+ /*!
+ *
+ * \brief Set Cartesian position of Hand (end-effector) in Global Frame (Euler Angles)
+ *
+ */
+ PR2::PR2_ERROR_CODE setHandCartesianPosition (double x, double y, double z, double roll, double pitch, double yaw);
- /*!
- *
- * \brief Set orientation of Hand (end-effector) in Global Frame (Euler Angles)
- *
- */
- PR2::PR2_ERROR_CODE setHandOrientation(double roll, double pitch, double yaw);
+ PR2::PR2_ERROR_CODE getHandCartesianPositionCmd(double *x, double *y, double *z, double *roll, double *pitch, double *yaw);
+ PR2::PR2_ERROR_CODE getHandCartesianPositionAct(double *x, double *y, double *z, double *roll, double *pitch, double *yaw);
- PR2::PR2_ERROR_CODE getHandOrientationCmd(double *roll, double *pitch, double *yaw);
- PR2::PR2_ERROR_CODE getHandOrientationAct(double *roll, double *pitch, double *yaw);
+ /*!
+ *
+ * \brief Set orientation of Hand (end-effector) in Global Frame (Euler Angles)
+ *
+ */
+ PR2::PR2_ERROR_CODE setHandOrientation(double roll, double pitch, double yaw);
- /*!
- *
- * \brief Set parameters for the hand (end-effector) controller
- *
- * user can set maximum velocity,
- * maximum acceleration, and
- * constraints for this controller
- *
- * e.g.: <br>
- * <UL type="none">
- * <LI> setParam('maxVel',10);
- * <LI> setParam('maxAcc',10);
- * </UL>
- *
- */
- PR2::PR2_ERROR_CODE setHandParam(string label, double value);
- PR2::PR2_ERROR_CODE setHandParam(string label, string value);
+ PR2::PR2_ERROR_CODE getHandOrientationCmd(double *roll, double *pitch, double *yaw);
+ PR2::PR2_ERROR_CODE getHandOrientationAct(double *roll, double *pitch, double *yaw);
- PR2::PR2_ERROR_CODE getHandParam(string label, double *value);
- PR2::PR2_ERROR_CODE getHandParam(string label, string *value);
+ /*!
+ *
+ * \brief Set parameters for the hand (end-effector) controller
+ *
+ * user can set maximum velocity,
+ * maximum acceleration, and
+ * constraints for this controller
+ *
+ * e.g.: <br>
+ * <UL type="none">
+ * <LI> setParam('maxVel',10);
+ * <LI> setParam('maxAcc',10);
+ * </UL>
+ *
+ */
+ PR2::PR2_ERROR_CODE setHandParam(string label, double value);
+ PR2::PR2_ERROR_CODE setHandParam(string label, string value);
- /*!
- *
- * \brief Set Arm joint positions individually.
- *
- */
- PR2::PR2_ERROR_CODE setArmJointPosition(int numJoints, double angles[],double speed[]);
+ PR2::PR2_ERROR_CODE getHandParam(string label, double *value);
+ PR2::PR2_ERROR_CODE getHandParam(string label, string *value);
- PR2::PR2_ERROR_CODE getArmJointPositionCmd(int *numJoints, double *angles[],double *speed[]);
- PR2::PR2_ERROR_CODE getArmJointPositionAct(int *numJoints, double *angles[],double *speed[]);
+ /*!
+ *
+ * \brief Set Arm joint positions individually.
+ *
+ */
+ PR2::PR2_ERROR_CODE setArmJointPosition(int numJoints, double angles[],double speed[]);
- /*!
- *
- * \brief Set Arm joint torques individually.
- *
- */
- PR2::PR2_ERROR_CODE setArmJointTorque(int numJoints, double torque[]);
+ PR2::PR2_ERROR_CODE getArmJointPositionCmd(int *numJoints, double *angles[],double *speed[]);
+ PR2::PR2_ERROR_CODE getArmJointPositionAct(int *numJoints, double *angles[],double *speed[]);
- PR2::PR2_ERROR_CODE getArmJointTorqueCmd(int *numJoints, double *torque[]);
- PR2::PR2_ERROR_CODE getArmJointTorqueAct(int *numJoints, double *torque[]);
+ /*!
+ *
+ * \brief Set Arm joint torques individually.
+ *
+ */
+ PR2::PR2_ERROR_CODE setArmJointTorque(int numJoints, double torque[]);
- /*!
- *
- * \brief Set Arm joint speeds individually.
- *
- */
- PR2::PR2_ERROR_CODE setArmJointSpeed(int numJoints, double speed[]);
+ PR2::PR2_ERROR_CODE getArmJointTorqueCmd(int *numJoints, double *torque[]);
+ PR2::PR2_ERROR_CODE getArmJointTorqueAct(int *numJoints, double *torque[]);
- PR2::PR2_ERROR_CODE getArmJointSpeedCmd(int *numJoints, double *speed[]);
- PR2::PR2_ERROR_CODE getArmJointSpeedAct(int *numJoints, double *speed[]);
+ /*!
+ *
+ * \brief Set Arm joint speeds individually.
+ *
+ */
+ PR2::PR2_ERROR_CODE setArmJointSpeed(int numJoints, double speed[]);
- /*!
- *
- * \brief Set arm joint maximum torques individually.
- *
- */
- PR2::PR2_ERROR_CODE setArmJointMaxTorque(int numJoints, double maxTorque[]);
+ PR2::PR2_ERROR_CODE getArmJointSpeedCmd(int *numJoints, double *speed[]);
+ PR2::PR2_ERROR_CODE getArmJointSpeedAct(int *numJoints, double *speed[]);
- PR2::PR2_ERROR_CODE getArmJointMaxTorqueCmd(int *numJoints, double *maxTorque[]);
- PR2::PR2_ERROR_CODE getArmJointMaxTorqueAct(int *numJoints, double *maxTorque[]);
+ /*!
+ *
+ * \brief Set arm joint maximum torques individually.
+ *
+ */
+ PR2::PR2_ERROR_CODE setArmJointMaxTorque(int numJoints, double maxTorque[]);
- /*!
- *
- * \brief Set forearm camera gazepoints.
- *
- * TODO: global frame???
- *
- */
- PR2::PR2_ERROR_CODE setArmCamGazePoint(double x, double y, double z);
+ PR2::PR2_ERROR_CODE getArmJointMaxTorqueCmd(int *numJoints, double *maxTorque[]);
+ PR2::PR2_ERROR_CODE getArmJointMaxTorqueAct(int *numJoints, double *maxTorque[]);
- PR2::PR2_ERROR_CODE getArmCamGazePointCmd(double *x, double *y, double *z);
- PR2::PR2_ERROR_CODE getArmCamGazePointAct(double *x, double *y, double *z);
+ /*!
+ *
+ * \brief Set forearm camera gazepoints.
+ *
+ * TODO: global frame???
+ *
+ */
+ PR2::PR2_ERROR_CODE setArmCamGazePoint(double x, double y, double z);
- private:
- PR2::PR2_CONTROL_MODE controlMode; /**< Arm controller control mode >*/
-};
+ PR2::PR2_ERROR_CODE getArmCamGazePointCmd(double *x, double *y, double *z);
+ PR2::PR2_ERROR_CODE getArmCamGazePointAct(double *x, double *y, double *z);
+ private:
+ PR2::PR2_CONTROL_MODE controlMode; /**< Arm controller control mode >*/
+ };
+}
+
Modified: pkg/trunk/controllers/pr2Controllers/include/pr2Controllers/BaseController.h
===================================================================
--- pkg/trunk/controllers/pr2Controllers/include/pr2Controllers/BaseController.h 2008-07-10 00:35:28 UTC (rev 1387)
+++ pkg/trunk/controllers/pr2Controllers/include/pr2Controllers/BaseController.h 2008-07-10 00:38:34 UTC (rev 1388)
@@ -22,106 +22,113 @@
#include <libpr2HW/pr2HW.h>
#include <genericControllers/Controller.h>
-
-class BaseController : CONTROLLER::Controller
+namespace CONTROLLER
{
- public:
-
- /*!
- * \brief Constructor of the BaseController class.
- *
- * \param
- */
- BaseController();
+ class BaseController : Controller
+ {
+ public:
- /*!
- * \brief Destructor of the BaseController class.
- */
- ~BaseController( );
+ /*!
+ * \brief Constructor of the BaseController class.
+ *
+ * \param
+ */
+ BaseController();
+
+ /*!
+ * \brief Destructor of the BaseController class.
+ */
+ ~BaseController( );
- /*!
- * \brief Drive robot on a course
- *
- * Give the course in the robot frame, with yaw=0 pointing forward.
- *
- * e.g. setting yaw=0 puts robot in a car-like mode.
- *
- * \param v The velocity of the robot.
- * \param yaw The angle of the robot x-axis relative to the globe x-axis.
- *
- */
- PR2::PR2_ERROR_CODE setCourse(double v , double yaw);
+ /*!
+ * \brief Update controller
+ */
+ void Update( );
- /*!
- * \brief Drive robot on a course in the Robot Frame
- *
- * Same as setCourse except the inputs are the x and y components of velocities.
- * \param xDot The velocity of the robot in the x-direction relative to the global frame.
- * \param yDot The velocity of the robot in the y-direction relative to the global frame.
- *
- * TODO: is setVelocity a good name?
- *
- */
- PR2::PR2_ERROR_CODE setVelocity(double xDot, double yDot);
+ /*!
+ * \brief Drive robot on a course
+ *
+ * Give the course in the robot frame, with yaw=0 pointing forward.
+ *
+ * e.g. setting yaw=0 puts robot in a car-like mode.
+ *
+ * \param v The velocity of the robot.
+ * \param yaw The angle of the robot x-axis relative to the globe x-axis.
+ *
+ */
+ PR2::PR2_ERROR_CODE setCourse(double v , double yaw);
- /*!
- * \brief Set target point in Global Frame
- *
- * TODO: this is a single point version of the setTrajectory function.
- *
- */
- PR2::PR2_ERROR_CODE setTarget(double x,double y, double yaw, double xDot, double yDot, double yawDot);
+ /*!
+ * \brief Drive robot on a course in the Robot Frame
+ *
+ * Same as setCourse except the inputs are the x and y components of velocities.
+ * \param xDot The velocity of the robot in the x-direction relative to the global frame.
+ * \param yDot The velocity of the robot in the y-direction relative to the global frame.
+ *
+ * TODO: is setVelocity a good name?
+ *
+ */
+ PR2::PR2_ERROR_CODE setVelocity(double xDot, double yDot);
- /*!
- * \brief Set target points (trajectory list) in Global Frame
- *
- * TODO: define design requirements, see meeting notes (Mechanism Control minutes 2008-06-30.
- *
- */
- PR2::PR2_ERROR_CODE setTraj(int numPts, double x[],double y[], double yaw[], double xDot[], double yDot[], double yawDot[]);
+ /*!
+ * \brief Set target point in Global Frame
+ *
+ * TODO: this is a single point version of the setTrajectory function.
+ *
+ */
+ PR2::PR2_ERROR_CODE setTarget(double x,double y, double yaw, double xDot, double yDot, double yawDot);
- /*!
- * \brief Heading pose for the robot
- *
- * Robot assume a stationary rotation mode, and achieve heading in Global Frame<br>
- * <UL type="none">
- * <LI> yaw=0 implies +x-axis direction,
- * <LI> +yaw implies counter-clockwise
- * </UL>
- *
- */
- PR2::PR2_ERROR_CODE setHeading(double yaw);
+ /*!
+ * \brief Set target points (trajectory list) in Global Frame
+ *
+ * TODO: define design requirements, see meeting notes (Mechanism Control minutes 2008-06-30.
+ *
+ */
+ PR2::PR2_ERROR_CODE setTraj(int numPts, double x[],double y[], double yaw[], double xDot[], double yDot[], double yawDot[]);
- /*!
- * \brief Set force (linear summation of contributions from all wheels) in global frame
- */
- PR2::PR2_ERROR_CODE setForce(double fx, double fy);
+ /*!
+ * \brief Heading pose for the robot
+ *
+ * Robot assume a stationary rotation mode, and achieve heading in Global Frame<br>
+ * <UL type="none">
+ * <LI> yaw=0 implies +x-axis direction,
+ * <LI> +yaw implies counter-clockwise
+ * </UL>
+ *
+ */
+ PR2::PR2_ERROR_CODE setHeading(double yaw);
- /*!
- * \brief Set wrench (linear summation of contributions from all wheels) in global frame
- *
- * Use to apply a wrenching torque usig the base wheels.
- *
- */
- PR2::PR2_ERROR_CODE setWrench(double yaw);
+ /*!
+ * \brief Set force (linear summation of contributions from all wheels) in global frame
+ */
+ PR2::PR2_ERROR_CODE setForce(double fx, double fy);
- /*!
- * \brief Set parameters for this controller
- *
- * user can set maximum velocity
- * and maximum acceleration
- * constraints for this controller
- *<br>
- *<UL TYPE="none">
- *<LI> e.g. setParam('maxVel',10);
- *<LI> or setParam('maxAcc',10);
- *</UL>
- *
- */
- PR2::PR2_ERROR_CODE setParam(string label,double value);
- PR2::PR2_ERROR_CODE setParam(string label,string value);
- private:
- PR2::PR2_CONTROL_MODE controlMode; /**< Base controller control mode >*/
-};
+ /*!
+ * \brief Set wrench (linear summation of contributions from all wheels) in global frame
+ *
+ * Use to apply a wrenching torque usig the base wheels.
+ *
+ */
+ PR2::PR2_ERROR_CODE setWrench(double yaw);
+ /*!
+ * \brief Set parameters for this controller
+ *
+ * user can set maximum velocity
+ * and maximum acceleration
+ * constraints for this controller
+ *<br>
+ *<UL TYPE="none">
+ *<LI> e.g. setParam('maxVel',10);
+ *<LI> or setParam('maxAcc',10);
+ *</UL>
+ *
+ */
+ PR2::PR2_ERROR_CODE setParam(string label,double value);
+ PR2::PR2_ERROR_CODE setParam(string label,string value);
+ private:
+ PR2::PR2_CONTROL_MODE controlMode; /**< Base controller control mode >*/
+ };
+}
+
Modified: pkg/trunk/controllers/pr2Controllers/include/pr2Controllers/GripperController.h
===================================================================
--- pkg/trunk/controllers/pr2Controllers/include/pr2Controllers/GripperController.h 2008-07-10 00:35:28 UTC (rev 1387)
+++ pkg/trunk/controllers/pr2Controllers/include/pr2Controllers/GripperController.h 2008-07-10 00:38:34 UTC (rev 1388)
@@ -18,64 +18,72 @@
#include <libpr2HW/pr2HW.h>
#include <genericControllers/Controller.h>
-class GripperController : CONTROLLER::Controller
+namespace CONTROLLER
{
- public:
-
- /*!
- * \brief Constructor of the GripperController class.
- *
- */
- GripperController();
+ class GripperController : Controller
+ {
+ public:
- /*!
- * \brief Destructor of the GripperController class.
- */
- ~GripperController( );
+ /*!
+ * \brief Constructor of the GripperController class.
+ *
+ */
+ GripperController();
+
+ /*!
+ * \brief Destructor of the GripperController class.
+ */
+ ~GripperController( );
- /*!
- * \brief Set gap between the two finger tips.
- *
- * \param distance The distance between the finger tips.
- */
- PR2::PR2_ERROR_CODE setGap(double distance);
+ /*!
+ * \brief Update controller
+ */
+ void Update( );
- /*!
- * \brief Set force between the two finger tips.
- *
- * \param force The force between the finger tips.
- */
- PR2::PR2_ERROR_CODE setForce(double force);
-
- /*!
- * \brief Close the gripper (force?).
- */
- PR2::PR2_ERROR_CODE close();
-
- /*!
- * \brief Open the gripper.
- */
- PR2::PR2_ERROR_CODE open();
+ /*!
+ * \brief Set gap between the two finger tips.
+ *
+ * \param distance The distance between the finger tips.
+ */
+ PR2::PR2_ERROR_CODE setGap(double distance);
- /*!
- * \brief Set parameters for this controller
- *
- * user can set maximum velocity
- * and maximum acceleration
- * constraints for this controller
- *<br>
- *<UL TYPE="none">
- *<LI> e.g. setParam('maxVel',10);
- *<LI> or setParam('maxAcc',10);
- *<LI> or setParam('maxGap',10);
- *<LI> or setParam('maxForce',10);
- *<LI> or setParam('closeForce',10);
- *</UL>
- */
- PR2::PR2_ERROR_CODE setParam(string label,double value);
- PR2::PR2_ERROR_CODE setParam(string label,string value);
- private:
- PR2::PR2_CONTROL_MODE controlMode; /**< Gripper controller control mode >*/
-};
+ /*!
+ * \brief Set force between the two finger tips.
+ *
+ * \param force The force between the finger tips.
+ */
+ PR2::PR2_ERROR_CODE setForce(double force);
+
+ /*!
+ * \brief Close the gripper (force?).
+ */
+ PR2::PR2_ERROR_CODE close();
+
+ /*!
+ * \brief Open the gripper.
+ */
+ PR2::PR2_ERROR_CODE open();
+ /*!
+ * \brief Set parameters for this controller
+ *
+ * user can set maximum velocity
+ * and maximum acceleration
+ * constraints for this controller
+ *<br>
+ *<UL TYPE="none">
+ *<LI> e.g. setParam('maxVel',10);
+ *<LI> or setParam('maxAcc',10);
+ *<LI> or setParam('maxGap',10);
+ *<LI> or setParam('maxForce',10);
+ *<LI> or setParam('closeForce',10);
+ *</UL>
+ */
+ PR2::PR2_ERROR_CODE setParam(string label,double value);
+ PR2::PR2_ERROR_CODE setParam(string label,string value);
+ private:
+ PR2::PR2_CONTROL_MODE controlMode; /**< Gripper controller control mode >*/
+ };
+}
+
Modified: pkg/trunk/controllers/pr2Controllers/include/pr2Controllers/HeadController.h
===================================================================
--- pkg/trunk/controllers/pr2Controllers/include/pr2Controllers/HeadController.h 2008-07-10 00:35:28 UTC (rev 1387)
+++ pkg/trunk/controllers/pr2Controllers/include/pr2Controllers/HeadController.h 2008-07-10 00:38:34 UTC (rev 1388)
@@ -22,78 +22,86 @@
#include <rosTF/rosTF.h>
-class HeadController : CONTROLLER::Controller
+namespace CONTROLLER
{
- public:
-
- /*!
- * \brief Constructor,
- *
- * \param
- */
- HeadController();
+ class HeadController : Controller
+ {
+ public:
- /*!
- * \brief Destructor of Pid class.
- */
- ~HeadController( );
+ /*!
+ * \brief Constructor,
+ *
+ * \param
+ */
+ HeadController();
+
+ /*!
+ * \brief Destructor of Pid class.
+ */
+ ~HeadController( );
- /*!
- * \brief Set yaw and pitch of head in Local Frames
- *
- * Angles are defined by robot description file.<br>
- * Angle of 0 is the home position.
- *
- */
- PR2::PR2_ERROR_CODE setPosition(double yaw , double pitch, double yawDot, double pitchDot);
+ /*!
+ * \brief Update controller
+ */
+ void Update( );
- /*!
- * \brief Set yaw rate and pitch rate for the head in Local Frames
- *
- */
- PR2::PR2_ERROR_CODE setPositionRate(double yawDot, double pitchDot);
+ /*!
+ * \brief Set yaw and pitch of head in Local Frames
+ *
+ * Angles are defined by robot description file.<br>
+ * Angle of 0 is the home position.
+ *
+ */
+ PR2::PR2_ERROR_CODE setPosition(double yaw , double pitch, double yawDot, double pitchDot);
- /*!
- * \brief Set gaze point in global frame
- *
- * omit xDot, yDot, zDot to denote 0 velocity at target point.
- *
- * TODO: fix how to pass transforms or keep track of them in the RT loop, efficiently.
- *
- */
- PR2::PR2_ERROR_CODE setGazePoint(double x,double y, double z, double xDot, double yDot, double zDot);
- PR2::PR2_ERROR_CODE setGazePoint(double x,double y, double z);
- PR2::PR2_ERROR_CODE setGazePoint(double x,double y, double z, rosTFServer tf);
+ /*!
+ * \brief Set yaw rate and pitch rate for the head in Local Frames
+ *
+ */
+ PR2::PR2_ERROR_CODE setPositionRate(double yawDot, double pitchDot);
- /*!
- * \brief Set saccading speed
- *
- * Not sure how to saccade yet.
- *
- */
- PR2::PR2_ERROR_CODE setSaccadeSpeed(double xDot, double yDot, double zDot);
+ /*!
+ * \brief Set gaze point in global frame
+ *
+ * omit xDot, yDot, zDot to denote 0 velocity at target point.
+ *
+ * TODO: fix how to pass transforms or keep track of them in the RT loop, efficiently.
+ *
+ */
+ PR2::PR2_ERROR_CODE setGazePoint(double x,double y, double z, double xDot, double yDot, double zDot);
+ PR2::PR2_ERROR_CODE setGazePoint(double x,double y, double z);
+ PR2::PR2_ERROR_CODE setGazePoint(double x,double y, double z, rosTFServer tf);
- /*!
- * \brief Set parameters for this controller
- *
- * user can set maximum velocity
- * and maximum acceleration
- * constraints for this controller
- *
- * e.g.: <br>
- * <UL type="none">
- * <LI> setParam('maxVel', 1.0);
- * <LI> setParam('maxAcc', 1.0);
- * <LI> setParam('maxLim', 1.57);
- * <LI> setParam('minLim',-1.57);
- * </UL>
- *
- */
- PR2::PR2_ERROR_CODE setParam(string label,double value);
- PR2::PR2_ERROR_CODE setParam(string label,string value);
+ /*!
+ * \brief Set saccading speed
+ *
+ * Not sure how to saccade yet.
+ *
+ */
+ PR2::PR2_ERROR_CODE setSaccadeSpeed(double xDot, double yDot, double zDot);
- private:
- PR2::PR2_CONTROL_MODE controlMode; /**< Head controller control mode >*/
-};
+ /*!
+ * \brief Set parameters for this controller
+ *
+ * user can set maximum velocity
+ * and maximum acceleration
+ * constraints for this controller
+ *
+ * e.g.: <br>
+ * <UL type="none">
+ * <LI> setParam('maxVel', 1.0);
+ * <LI> setParam('maxAcc', 1.0);
+ * <LI> setParam('maxLim', 1.57);
+ * <LI> setParam('minLim',-1.57);
+ * </UL>
+ *
+ */
+ PR2::PR2_ERROR_CODE setParam(string label,double value);
+ PR2::PR2_ERROR_CODE setParam(string label,string value);
+ private:
+ PR2::PR2_CONTROL_MODE controlMode; /**< Head controller control mode >*/
+ };
+}
+
Modified: pkg/trunk/controllers/pr2Controllers/include/pr2Controllers/LaserScannerController.h
===================================================================
--- pkg/trunk/controllers/pr2Controllers/include/pr2Controllers/LaserScannerController.h 2008-07-10 00:35:28 UTC (rev 1387)
+++ pkg/trunk/controllers/pr2Controllers/include/pr2Controllers/LaserScannerController.h 2008-07-10 00:38:34 UTC (rev 1388)
@@ -20,65 +20,72 @@
#include <libpr2HW/pr2HW.h>
#include <genericControllers/Controller.h>
-
-class LaserScannerController : CONTROLLER::Controller
+namespace CONTROLLER
{
- public:
-
- /*!
- * \brief Constructor.
- *
- * \param
- */
- LaserScannerController();
+ class LaserScannerController : Controller
+ {
+ public:
- /*!
- * \brief Destructor.
- */
- ~LaserScannerController( );
+ /*!
+ * \brief Constructor.
+ *
+ * \param
+ */
+ LaserScannerController();
+
+ /*!
+ * \brief Destructor.
+ */
+ ~LaserScannerController( );
- /*!
- * \brief Set scanning profile for the pitching Hokuyo
- *
- * To be determined on how to implement
- * For now, use sawtooth or sine wave
- * Consider also, using setParam('profile','sawtooth')
- *
- */
- PR2::PR2_ERROR_CODE setProfile(double *t, double *x);
+ /*!
+ * \brief Update controller
+ */
+ void Update( );
- /*!
- * \brief Set pitch angle of the pitching Hokuyo joint
- *
- * pitch=0 means home position of horizontal scan.
- *
- */
- PR2::PR2_ERROR_CODE SetPosition(double pitch);
+ /*!
+ * \brief Set scanning profile for the pitching Hokuyo
+ *
+ * To be determined on how to implement
+ * For now, use sawtooth or sine wave
+ * Consider also, using setParam('profile','sawtooth')
+ *
+ */
+ PR2::PR2_ERROR_CODE setProfile(double *t, double *x);
- /*!
- * \brief Set parameters for this controller
- *
- * user can set maximum velocity
- * and maximum acceleration
- * constraints for this controller
- *
- * e.g. <br>
- * <UL type="none">
- * <LI> setParam('maxVel', 1.0);
- * <LI> setParam('maxAcc', 1.0);
- * <LI> setParam('profile','sawtooth');
- * <LI> setParam('profile','sinewave');
- * <LI> setParam('maxLim', 1.0);
- * <LI> setParam('minLim',-1.0);
- * <LI> setParam('rateHz' , 1);
- * </UL>
- *
- */
- PR2::PR2_ERROR_CODE setParam(string label,double value);
- PR2::PR2_ERROR_CODE setParam(string label,string value);
+ /*!
+ * \brief Set pitch angle of the pitching Hokuyo joint
+ *
+ * pitch=0 means home position of horizontal scan.
+ *
+ */
+ PR2::PR2_ERROR_CODE SetPosition(double pitch);
- private:
- PR2::PR2_CONTROL_MODE controlMode; /**< Pitching Hokuyo laser scanner controller control mode >*/
-};
+ /*!
+ * \brief Set parameters for this controller
+ *
+ * user can set maximum velocity
+ * and maximum acceleration
+ * constraints for this controller
+ *
+ * e.g. <br>
+ * <UL type="none">
+ * <LI> setParam('maxVel', 1.0);
+ * <LI> setParam('maxAcc', 1.0);
+ * <LI> setParam('profile','sawtooth');
+ * <LI> setParam('profile','sinewave');
+ * <LI> setParam('maxLim', 1.0);
+ * <LI> setParam('minLim',-1.0);
+ * <LI> setParam('rateHz' , 1);
+ * </UL>
+ *
+ */
+ PR2::PR2_ERROR_CODE setParam(string label,double value);
+ PR2::PR2_ERROR_CODE setParam(string label,string value);
+ private:
+ PR2::PR2_CONTROL_MODE controlMode; /**< Pitching Hokuyo laser scanner controller control mode >*/
+ };
+}
+
Modified: pkg/trunk/controllers/pr2Controllers/include/pr2Controllers/SpineController.h
===================================================================
--- pkg/trunk/controllers/pr2Controllers/include/pr2Controllers/SpineController.h 2008-07-10 00:35:28 UTC (rev 1387)
+++ pkg/trunk/controllers/pr2Controllers/include/pr2Controllers/SpineController.h 2008-07-10 00:38:34 UTC (rev 1388)
@@ -20,52 +20,58 @@
#include <libpr2HW/pr2HW.h>
#include <genericControllers/Controller.h>
-
-class SpineController : CONTROLLER::Controller
+namespace CONTROLLER
{
- public:
-
- /*!
- * \bri...
[truncated message content] |
|
From: <hsu...@us...> - 2008-07-10 02:03:30
|
Revision: 1396
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=1396&view=rev
Author: hsujohnhsu
Date: 2008-07-09 19:03:40 -0700 (Wed, 09 Jul 2008)
Log Message:
-----------
clean up some printf statements.
Modified Paths:
--------------
pkg/trunk/drivers/robot/pr2/libpr2GZ/src/pr2GZ.cpp
pkg/trunk/drivers/robot/pr2/libpr2HW/src/pr2HW.cpp
pkg/trunk/drivers/robot/pr2/pr2Core/include/pr2Core/pr2Misc.h
pkg/trunk/simulators/pr2MechCon/pr2MechCon.cc
Modified: pkg/trunk/drivers/robot/pr2/libpr2GZ/src/pr2GZ.cpp
===================================================================
--- pkg/trunk/drivers/robot/pr2/libpr2GZ/src/pr2GZ.cpp 2008-07-10 02:02:17 UTC (rev 1395)
+++ pkg/trunk/drivers/robot/pr2/libpr2GZ/src/pr2GZ.cpp 2008-07-10 02:03:40 UTC (rev 1396)
@@ -360,12 +360,6 @@
PR2_ERROR_CODE PR2GZ::SetJointControlMode(PR2_JOINT_ID id, PR2_JOINT_CONTROL_MODE mode)
{
- printf("Called SetJointControlMode\n");
- printf("id: %d, mode: %d\n", id, mode);
- printf("IsHead: %d\n", IsHead(id));
- printf("IsGripperLeft: %d\n", IsGripperLeft(id));
- printf("IsGripperRight: %d\n", IsGripperRight(id));
-
if(IsHead(id))
{
pr2HeadIface->Lock(1);
@@ -387,7 +381,6 @@
else
{
pr2Iface->Lock(1);
- printf("id: %d, mode: %d\n", id, mode);
pr2Iface->data->actuators[id].controlMode = mode;
pr2Iface->Unlock();
}
Modified: pkg/trunk/drivers/robot/pr2/libpr2HW/src/pr2HW.cpp
===================================================================
--- pkg/trunk/drivers/robot/pr2/libpr2HW/src/pr2HW.cpp 2008-07-10 02:02:17 UTC (rev 1395)
+++ pkg/trunk/drivers/robot/pr2/libpr2HW/src/pr2HW.cpp 2008-07-10 02:03:40 UTC (rev 1396)
@@ -360,12 +360,6 @@
PR2_ERROR_CODE PR2HW::SetJointControlMode(PR2_JOINT_ID id, PR2_JOINT_CONTROL_MODE mode)
{
- printf("Called SetJointControlMode\n");
- printf("id: %d, mode: %d\n", id, mode);
- printf("IsHead: %d\n", IsHead(id));
- printf("IsGripperLeft: %d\n", IsGripperLeft(id));
- printf("IsGripperRight: %d\n", IsGripperRight(id));
-
if(IsHead(id))
{
pr2HeadIface->Lock(1);
@@ -387,7 +381,6 @@
else
{
pr2Iface->Lock(1);
- printf("id: %d, mode: %d\n", id, mode);
pr2Iface->data->actuators[id].controlMode = mode;
pr2Iface->Unlock();
}
Modified: pkg/trunk/drivers/robot/pr2/pr2Core/include/pr2Core/pr2Misc.h
===================================================================
--- pkg/trunk/drivers/robot/pr2/pr2Core/include/pr2Core/pr2Misc.h 2008-07-10 02:02:17 UTC (rev 1395)
+++ pkg/trunk/drivers/robot/pr2/pr2Core/include/pr2Core/pr2Misc.h 2008-07-10 02:03:40 UTC (rev 1396)
@@ -55,7 +55,6 @@
inline bool IsGripperLeft(PR2_JOINT_ID id)
{
- printf("in IsGripperLeft:\nid: %d, Start: %d, end:%d\n",id,JointStart[PR2_LEFT_GRIPPER],JointEnd[PR2_LEFT_GRIPPER]);
if (id >= JointStart[PR2_LEFT_GRIPPER] && id <= JointEnd[PR2_LEFT_GRIPPER])
return true;
return false;
Modified: pkg/trunk/simulators/pr2MechCon/pr2MechCon.cc
===================================================================
--- pkg/trunk/simulators/pr2MechCon/pr2MechCon.cc 2008-07-10 02:02:17 UTC (rev 1395)
+++ pkg/trunk/simulators/pr2MechCon/pr2MechCon.cc 2008-07-10 02:03:40 UTC (rev 1396)
@@ -28,7 +28,6 @@
#include <gazebo/GazeboError.hh>
#include <libpr2API/pr2API.h>
-#include <genericControllers/Controller.h>
#include <pr2Controllers/ArmController.h>
#include <pr2Controllers/HeadController.h>
#include <pr2Controllers/SpineController.h>
@@ -146,9 +145,6 @@
// keep count for full cloud
int max_cloud_pts;
-
- // clean up on interrupt
- static void finalize(int);
};
void
@@ -310,7 +306,7 @@
}
-void RosGazeboNode::finalize(int)
+void finalize(int)
{
fprintf(stderr,"Caught sig, clean-up and exit\n");
sleep(1);
@@ -1108,6 +1104,7 @@
void *nonRealtimeLoop(void *rgn)
{
+ std::cout << "Started nonRT loop" << std::endl;
while (1)
{
((RosGazeboNode*)rgn)->Update();
@@ -1192,9 +1189,9 @@
/* on termination... */
/* */
/***************************************************************************************/
- signal(SIGINT, (&rgn.finalize));
- signal(SIGQUIT, (&rgn.finalize));
- signal(SIGTERM, (&rgn.finalize));
+ signal(SIGINT, (&finalize));
+ signal(SIGQUIT, (&finalize));
+ signal(SIGTERM, (&finalize));
// see if we can subscribe models needed
if (rgn.AdvertiseSubscribeMessages() != 0)
@@ -1209,7 +1206,7 @@
int rgnt = pthread_create(&threads[0],NULL, nonRealtimeLoop, (void *) (&rgn));
if (rgnt)
{
- printf("Could not start ROSGazeboNode (code=%d)\n",rgnt);
+ printf("Could not start a separate thread for ROS Gazebo Node (code=%d)\n",rgnt);
exit(-1);
}
@@ -1223,6 +1220,8 @@
{
// Update Controllers
+ // each controller will try to read new commands from shared memory with nonRT hooks,
+ // and skip update if locked by nonRT loop.
myArm.Update();
myHead.Update();
mySpine.Update();
@@ -1230,6 +1229,8 @@
myLaserScanner.Update();
myGripper.Update();
+ // TODO: Safety codes should go here...
+
// Send updated controller commands to hardware
// myPR2->hw.UpdateHW();
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <hsu...@us...> - 2008-07-10 18:18:16
|
Revision: 1409
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=1409&view=rev
Author: hsujohnhsu
Date: 2008-07-10 11:18:15 -0700 (Thu, 10 Jul 2008)
Log Message:
-----------
move demo to run pr2MechCon using rosgazebonode as a library.
Modified Paths:
--------------
pkg/trunk/demos/2dnav-gazebo/2dnav-gazebo.xml
Added Paths:
-----------
pkg/trunk/simulators/pr2MechCon/run-gazebo.sh
pkg/trunk/simulators/pr2MechCon/run-pr2MechCon.sh
pkg/trunk/simulators/pr2MechCon/setup.bash
pkg/trunk/simulators/pr2MechCon/setup.tcsh
Modified: pkg/trunk/demos/2dnav-gazebo/2dnav-gazebo.xml
===================================================================
--- pkg/trunk/demos/2dnav-gazebo/2dnav-gazebo.xml 2008-07-10 18:10:19 UTC (rev 1408)
+++ pkg/trunk/demos/2dnav-gazebo/2dnav-gazebo.xml 2008-07-10 18:18:15 UTC (rev 1409)
@@ -1,9 +1,9 @@
<launch>
<group name="wg">
- <node pkg="rosgazebo" type="run-gazebo.sh" args="$(find 2dnav-gazebo)/world/robot.world" respawn="true" />
+ <node pkg="pr2MechCon" type="run-gazebo.sh" args="$(find 2dnav-gazebo)/world/robot.world" respawn="true" />
<node pkg="map_server" type="map_server" args="$(find 2dnav-gazebo)/world/Media/materials/textures/map3.png 0.1" respawn="false" />
- <node pkg="rosgazebo" type="run-rosgazebo.sh" args="" respawn="true" />
+ <node pkg="pr2MechCon" type="run-pr2MechCon.sh" args="" respawn="true" />
<node pkg="amcl_player" type="amcl_player" respawn="false" />
<node pkg="wavefront_player" type="wavefront_player" respawn="false" />
<node pkg="nav_view" type="nav_view" respawn="true" />
Added: pkg/trunk/simulators/pr2MechCon/run-gazebo.sh
===================================================================
--- pkg/trunk/simulators/pr2MechCon/run-gazebo.sh (rev 0)
+++ pkg/trunk/simulators/pr2MechCon/run-gazebo.sh 2008-07-10 18:18:15 UTC (rev 1409)
@@ -0,0 +1,3 @@
+#!/bin/bash
+. ./setup.bash
+gazebo $*
Property changes on: pkg/trunk/simulators/pr2MechCon/run-gazebo.sh
___________________________________________________________________
Name: svn:executable
+ *
Added: pkg/trunk/simulators/pr2MechCon/run-pr2MechCon.sh
===================================================================
--- pkg/trunk/simulators/pr2MechCon/run-pr2MechCon.sh (rev 0)
+++ pkg/trunk/simulators/pr2MechCon/run-pr2MechCon.sh 2008-07-10 18:18:15 UTC (rev 1409)
@@ -0,0 +1,3 @@
+#!/bin/bash
+. `rospack find pr2MechCon`/setup.bash
+`rospack find pr2MechCon`/pr2MechCon $*
Property changes on: pkg/trunk/simulators/pr2MechCon/run-pr2MechCon.sh
___________________________________________________________________
Name: svn:executable
+ *
Added: pkg/trunk/simulators/pr2MechCon/setup.bash
===================================================================
--- pkg/trunk/simulators/pr2MechCon/setup.bash (rev 0)
+++ pkg/trunk/simulators/pr2MechCon/setup.bash 2008-07-10 18:18:15 UTC (rev 1409)
@@ -0,0 +1,33 @@
+#!/bin/bash
+export GAZ_TOP=`rospack find gazebo`/gazebo
+export OGRE_TOP=`rospack find ogre`/ogre
+export FREEIMAGE_TOP=`rospack find freeimage`/freeimage
+export CG_TOP=`rospack find Cg`/Cg
+export SIM_PLUGIN=`rospack find gazebo_plugin`
+export PR2API=`rospack find libpr2API`
+export PR2HW=`rospack find libpr2HW`
+export PR2MEDIA=`rospack find 2dnav-gazebo`/world
+
+
+export LD_LIBRARY_PATH=''
+export LD_LIBRARY_PATH=$PR2API/lib:$LD_LIBRARY_PATH
+export LD_LIBRARY_PATH=$PR2HW/lib:$LD_LIBRARY_PATH
+export LD_LIBRARY_PATH=$SIM_PLUGIN/lib:$LD_LIBRARY_PATH
+export LD_LIBRARY_PATH=$GAZ_TOP/lib:$LD_LIBRARY_PATH
+export LD_LIBRARY_PATH=$OGRE_TOP/lib:$LD_LIBRARY_PATH
+export LD_LIBRARY_PATH=$OGRE_TOP/lib/OGRE:$LD_LIBRARY_PATH
+export LD_LIBRARY_PATH=$FREEIMAGE_TOP/lib:$LD_LIBRARY_PATH
+export LD_LIBRARY_PATH=$CG_TOP/lib:$LD_LIBRARY_PATH
+export LD_LIBRARY_PATH=$SIM_PLUGIN/lib:$LD_LIBRARY_PATH
+export PATH=$GAZ_TOP/bin:$PATH
+
+export GAZEBO_RESOURCE_PATH=$PR2MEDIA
+export OGRE_RESOURCE_PATH=$OGRE_TOP/lib/OGRE
+
+echo
+echo Current GAZ_TOP is set to $GAZ_TOP
+echo Paths have been set accordingly.
+echo
+echo Now run gazebo [...world file...]
+echo
+
Property changes on: pkg/trunk/simulators/pr2MechCon/setup.bash
___________________________________________________________________
Name: svn:executable
+ *
Added: pkg/trunk/simulators/pr2MechCon/setup.tcsh
===================================================================
--- pkg/trunk/simulators/pr2MechCon/setup.tcsh (rev 0)
+++ pkg/trunk/simulators/pr2MechCon/setup.tcsh 2008-07-10 18:18:15 UTC (rev 1409)
@@ -0,0 +1,33 @@
+#!/bin/tcsh
+setenv GAZ_TOP `rospack find gazebo`/gazebo
+setenv OGRE_TOP `rospack find ogre`/ogre
+setenv FREEIMAGE_TOP `rospack find freeimage`/freeimage
+setenv CG_TOP `rospack find Cg`/Cg
+setenv SIM_PLUGIN `rospack find gazebo_plugin`
+setenv PR2API `rospack find libpr2API`
+setenv PR2HW `rospack find libpr2HW`
+setenv PR2MEDIA `rospack find 2dnav-gazebo`/world
+
+if (! $?LD_LIBRARY_PATH) setenv LD_LIBRARY_PATH ''
+setenv LD_LIBRARY_PATH ''
+setenv LD_LIBRARY_PATH $PR2API/lib:$LD_LIBRARY_PATH
+setenv LD_LIBRARY_PATH $PR2HW/lib:$LD_LIBRARY_PATH
+setenv LD_LIBRARY_PATH $SIM_PLUGIN/lib:$LD_LIBRARY_PATH
+setenv LD_LIBRARY_PATH $GAZ_TOP/lib:$LD_LIBRARY_PATH
+setenv LD_LIBRARY_PATH $OGRE_TOP/lib:$LD_LIBRARY_PATH
+setenv LD_LIBRARY_PATH $OGRE_TOP/lib/OGRE:$LD_LIBRARY_PATH
+setenv LD_LIBRARY_PATH $FREEIMAGE_TOP/lib:$LD_LIBRARY_PATH
+setenv LD_LIBRARY_PATH $CG_TOP/lib:$LD_LIBRARY_PATH
+setenv LD_LIBRARY_PATH $SIM_PLUGIN/lib:$LD_LIBRARY_PATH
+setenv PATH $GAZ_TOP/bin:$PATH
+
+setenv GAZEBO_RESOURCE_PATH $PR2MEDIA
+setenv OGRE_RESOURCE_PATH $OGRE_TOP/lib/OGRE
+
+echo
+echo Current GAZ_TOP is set to $GAZ_TOP
+echo Paths have been set accordingly.
+echo
+echo Now run gazebo [...world file...]
+echo
+
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <hsu...@us...> - 2008-07-10 19:28:03
|
Revision: 1416
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=1416&view=rev
Author: hsujohnhsu
Date: 2008-07-10 12:28:11 -0700 (Thu, 10 Jul 2008)
Log Message:
-----------
added damping for hinge joints in torque control and pd_torque control modes.
Modified Paths:
--------------
pkg/trunk/3rdparty/gazebo/gazebo_patch.diff
pkg/trunk/demos/2dnav-gazebo/world/robot_torque.world
pkg/trunk/simulators/gazebo_plugin/include/gazebo_plugin/Pr2_Actarray.hh
pkg/trunk/simulators/gazebo_plugin/src/Pr2_Actarray.cc
Modified: pkg/trunk/3rdparty/gazebo/gazebo_patch.diff
===================================================================
--- pkg/trunk/3rdparty/gazebo/gazebo_patch.diff 2008-07-10 19:14:58 UTC (rev 1415)
+++ pkg/trunk/3rdparty/gazebo/gazebo_patch.diff 2008-07-10 19:28:11 UTC (rev 1416)
@@ -230,7 +230,7 @@
};
/// \brief Gripper interface
-@@ -1254,9 +1304,327 @@
+@@ -1254,9 +1304,328 @@
/// \} */
@@ -483,6 +483,7 @@
+ double dGain;
+ double iClamp;
+ double saturationTorque;
++ double dampingCoefficient;
+ } JointData;
+
+/// \brief The actuator array data packet.
@@ -558,7 +559,7 @@
/** \defgroup ptz_iface ptz
\brief PTZ interface
-@@ -1334,7 +1702,7 @@
+@@ -1334,7 +1703,7 @@
\{
*/
@@ -567,7 +568,7 @@
#define GAZEBO_STEREO_CAMERA_MAX_DISPARITY_SIZE 640 * 480
/// \brief Stereo data
-@@ -1384,6 +1752,9 @@
+@@ -1384,6 +1753,9 @@
/// Right disparity (float)
public: float right_disparity[GAZEBO_STEREO_CAMERA_MAX_DISPARITY_SIZE];
@@ -577,7 +578,7 @@
};
-@@ -1401,6 +1772,7 @@
+@@ -1401,6 +1773,7 @@
{
Iface::Create(server,id);
this->data = (StereoCameraData*)this->mMap;
@@ -585,7 +586,7 @@
}
/// \brief Open the iface
-@@ -1408,8 +1780,16 @@
+@@ -1408,8 +1781,16 @@
{
Iface::Open(client,id);
this->data = (StereoCameraData*)this->mMap;
Modified: pkg/trunk/demos/2dnav-gazebo/world/robot_torque.world
===================================================================
--- pkg/trunk/demos/2dnav-gazebo/world/robot_torque.world 2008-07-10 19:14:58 UTC (rev 1415)
+++ pkg/trunk/demos/2dnav-gazebo/world/robot_torque.world 2008-07-10 19:28:11 UTC (rev 1416)
@@ -369,6 +369,7 @@
<dGain>0</dGain>
<iGain>0</iGain>
<iClamp>0</iClamp>
+ <explicitDampingCoefficient>1.0</explicitDampingCoefficient>
</joint>
<joint name="head_tilt_joint">
<saturationTorque>10</saturationTorque>
@@ -376,6 +377,7 @@
<dGain>0</dGain>
<iGain>0</iGain>
<iClamp>0</iClamp>
+ <explicitDampingCoefficient>1.0</explicitDampingCoefficient>
</joint>
<joint name="hokuyo_pitch_joint">
<saturationTorque>1000</saturationTorque>
@@ -383,6 +385,7 @@
<dGain>0</dGain>
<iGain>0</iGain>
<iClamp>0</iClamp>
+ <explicitDampingCoefficient>1.0</explicitDampingCoefficient>
</joint>
<joint name="head_cam_left_yaw_joint">
<saturationTorque>100</saturationTorque>
@@ -390,6 +393,7 @@
<dGain>0</dGain>
<iGain>0</iGain>
<iClamp>0</iClamp>
+ <explicitDampingCoefficient>1.0</explicitDampingCoefficient>
</joint>
<joint name="head_cam_left_pitch_joint">
<saturationTorque>100</saturationTorque>
@@ -397,6 +401,7 @@
<dGain>0</dGain>
<iGain>0</iGain>
<iClamp>0</iClamp>
+ <explicitDampingCoefficient>1.0</explicitDampingCoefficient>
</joint>
<joint name="head_cam_right_yaw_joint">
<saturationTorque>100</saturationTorque>
@@ -404,6 +409,7 @@
<dGain>0</dGain>
<iGain>0</iGain>
<iClamp>0</iClamp>
+ <explicitDampingCoefficient>1.0</explicitDampingCoefficient>
</joint>
<joint name="head_cam_right_pitch_joint">
<saturationTorque>100</saturationTorque>
@@ -411,6 +417,7 @@
<dGain>0</dGain>
<iGain>0</iGain>
<iClamp>0</iClamp>
+ <explicitDampingCoefficient>1.0</explicitDampingCoefficient>
</joint>
<interface:pr2array name="pr2_head_iface"/>
</controller:pr2_actarray>
@@ -425,6 +432,7 @@
<dGain>0</dGain>
<iGain>0</iGain>
<iClamp>0</iClamp>
+ <explicitDampingCoefficient>1.0</explicitDampingCoefficient>
</joint>
<joint name="front_left_wheel_l_drive">
<saturationTorque>5</saturationTorque>
@@ -432,6 +440,7 @@
<dGain>0</dGain>
<iGain>0</iGain>
<iClamp>0</iClamp>
+ <explicitDampingCoefficient>1.0</explicitDampingCoefficient>
</joint>
<joint name="front_left_wheel_r_drive">
<saturationTorque>5</saturationTorque>
@@ -439,6 +448,7 @@
<dGain>0</dGain>
<iGain>0</iGain>
<iClamp>0</iClamp>
+ <explicitDampingCoefficient>1.0</explicitDampingCoefficient>
</joint>
<joint name="front_right_caster_steer">
<saturationTorque>1000</saturationTorque>
@@ -446,6 +456,7 @@
<dGain>0</dGain>
<iGain>0</iGain>
<iClamp>0</iClamp>
+ <explicitDampingCoefficient>1.0</explicitDampingCoefficient>
</joint>
<joint name="front_right_wheel_l_drive">
<saturationTorque>5</saturationTorque>
@@ -453,6 +464,7 @@
<dGain>0</dGain>
<iGain>0</iGain>
<iClamp>0</iClamp>
+ <explicitDampingCoefficient>1.0</explicitDampingCoefficient>
</joint>
<joint name="front_right_wheel_r_drive">
<saturationTorque>5</saturationTorque>
@@ -460,6 +472,7 @@
<dGain>0</dGain>
<iGain>0</iGain>
<iClamp>0</iClamp>
+ <explicitDampingCoefficient>1.0</explicitDampingCoefficient>
</joint>
<joint name="rear_left_caster_steer">
<saturationTorque>1000</saturationTorque>
@@ -467,6 +480,7 @@
<dGain>0</dGain>
<iGain>0</iGain>
<iClamp>0</iClamp>
+ <explicitDampingCoefficient>1.0</explicitDampingCoefficient>
</joint>
<joint name="rear_left_wheel_l_drive">
<saturationTorque>5</saturationTorque>
@@ -474,6 +488,7 @@
<dGain>0</dGain>
<iGain>0</iGain>
<iClamp>0</iClamp>
+ <explicitDampingCoefficient>1.0</explicitDampingCoefficient>
</joint>
<joint name="rear_left_wheel_r_drive">
<saturationTorque>5</saturationTorque>
@@ -481,6 +496,7 @@
<dGain>0</dGain>
<iGain>0</iGain>
<iClamp>0</iClamp>
+ <explicitDampingCoefficient>1.0</explicitDampingCoefficient>
</joint>
<joint name="rear_right_caster_steer">
<saturationTorque>1000</saturationTorque>
@@ -488,6 +504,7 @@
<dGain>0</dGain>
<iGain>0</iGain>
<iClamp>0</iClamp>
+ <explicitDampingCoefficient>1.0</explicitDampingCoefficient>
</joint>
<joint name="rear_right_wheel_l_drive">
<saturationTorque>5</saturationTorque>
@@ -495,6 +512,7 @@
<dGain>0</dGain>
<iGain>0</iGain>
<iClamp>0</iClamp>
+ <explicitDampingCoefficient>1.0</explicitDampingCoefficient>
</joint>
<joint name="rear_right_wheel_r_drive">
<saturationTorque>5</saturationTorque>
@@ -502,6 +520,7 @@
<dGain>0</dGain>
<iGain>0</iGain>
<iClamp>0</iClamp>
+ <explicitDampingCoefficient>1.0</explicitDampingCoefficient>
</joint>
<!-- ========================================= -->
<joint name="torso_spine_slider">
@@ -510,6 +529,7 @@
<dGain>0</dGain>
<iGain>0</iGain>
<iClamp>0</iClamp>
+ <explicitDampingCoefficient>1.0</explicitDampingCoefficient>
</joint>
<!-- ========================================= -->
<joint name="shoulder_pan_left">
@@ -518,6 +538,7 @@
<dGain>0</dGain>
<iGain>500</iGain>
<iClamp>500</iClamp>
+ <explicitDampingCoefficient>10.0</explicitDampingCoefficient>
<controlMode>PD_TORQUE_CONTROL</controlMode>
</joint>
<joint name="upperarm_pitch_joint_left">
@@ -526,6 +547,7 @@
<dGain>0</dGain>
<iGain>500</iGain>
<iClamp>500</iClamp>
+ <explicitDampingCoefficient>100.0</explicitDampingCoefficient>
<controlMode>PD_TORQUE_CONTROL</controlMode>
</joint>
<joint name="upperarm_roll_joint_left">
@@ -534,6 +556,7 @@
<dGain>0</dGain>
<iGain>10</iGain>
<iClamp>10</iClamp>
+ <explicitDampingCoefficient>2.0</explicitDampingCoefficient>
<controlMode>PD_TORQUE_CONTROL</controlMode>
</joint>
<joint name="elbow_pitch_joint_left">
@@ -542,6 +565,7 @@
<dGain>0</dGain>
<iGain>100</iGain>
<iClamp>100</iClamp>
+ <explicitDampingCoefficient>2.0</explicitDampingCoefficient>
<controlMode>PD_TORQUE_CONTROL</controlMode>
</joint>
<joint name="forearm_roll_joint_left">
@@ -550,6 +574,7 @@
<dGain>0</dGain>
<iGain>1</iGain>
<iClamp>1</iClamp>
+ <explicitDampingCoefficient>2.0</explicitDampingCoefficient>
<controlMode>PD_TORQUE_CONTROL</controlMode>
</joint>
<joint name="wrist_pitch_joint_left">
@@ -558,6 +583,7 @@
<dGain>0</dGain>
<iGain>10</iGain>
<iClamp>10</iClamp>
+ <explicitDampingCoefficient>2.0</explicitDampingCoefficient>
<controlMode>PD_TORQUE_CONTROL</controlMode>
</joint>
<joint name="palm_roll_joint_left">
@@ -566,6 +592,7 @@
<dGain>0</dGain>
<iGain>1</iGain>
<iClamp>1</iClamp>
+ <explicitDampingCoefficient>2.0</explicitDampingCoefficient>
<controlMode>PD_TORQUE_CONTROL</controlMode>
</joint>
<!-- ========================================= -->
@@ -575,6 +602,7 @@
<dGain>0</dGain>
<iGain>500</iGain>
<iClamp>500</iClamp>
+ <explicitDampingCoefficient>10.0</explicitDampingCoefficient>
<controlMode>PD_TORQUE_CONTROL</controlMode>
</joint>
<joint name="upperarm_pitch_joint_right">
@@ -583,6 +611,7 @@
<dGain>0</dGain>
<iGain>500</iGain>
<iClamp>500</iClamp>
+ <explicitDampingCoefficient>100.0</explicitDampingCoefficient>
<controlMode>PD_TORQUE_CONTROL</controlMode>
</joint>
<joint name="upperarm_roll_joint_right">
@@ -591,6 +620,7 @@
<dGain>0</dGain>
<iGain>10</iGain>
<iClamp>10</iClamp>
+ <explicitDampingCoefficient>1.0</explicitDampingCoefficient>
<controlMode>PD_TORQUE_CONTROL</controlMode>
</joint>
<joint name="elbow_pitch_joint_right">
@@ -599,6 +629,7 @@
<dGain>0</dGain>
<iGain>100</iGain>
<iClamp>100</iClamp>
+ <explicitDampingCoefficient>1.0</explicitDampingCoefficient>
<controlMode>PD_TORQUE_CONTROL</controlMode>
</joint>
<joint name="forearm_roll_joint_right">
@@ -607,6 +638,7 @@
<dGain>0</dGain>
<iGain>1</iGain>
<iClamp>1</iClamp>
+ <explicitDampingCoefficient>1.0</explicitDampingCoefficient>
<controlMode>PD_TORQUE_CONTROL</controlMode>
</joint>
<joint name="wrist_pitch_joint_right">
@@ -615,6 +647,7 @@
<dGain>0</dGain>
<iGain>10</iGain>
<iClamp>10</iClamp>
+ <explicitDampingCoefficient>1.0</explicitDampingCoefficient>
<controlMode>PD_TORQUE_CONTROL</controlMode>
</joint>
<joint name="palm_roll_joint_right">
@@ -623,6 +656,7 @@
<dGain>0</dGain>
<iGain>1</iGain>
<iClamp>1</iClamp>
+ <explicitDampingCoefficient>1.0</explicitDampingCoefficient>
<controlMode>PD_TORQUE_CONTROL</controlMode>
</joint>
<!-- ========================================= -->
Modified: pkg/trunk/simulators/gazebo_plugin/include/gazebo_plugin/Pr2_Actarray.hh
===================================================================
--- pkg/trunk/simulators/gazebo_plugin/include/gazebo_plugin/Pr2_Actarray.hh 2008-07-10 19:14:58 UTC (rev 1415)
+++ pkg/trunk/simulators/gazebo_plugin/include/gazebo_plugin/Pr2_Actarray.hh 2008-07-10 19:28:11 UTC (rev 1416)
@@ -99,6 +99,9 @@
// save last time for dt calc for pid's
private: double lastTime;
+ // explicit damping coefficient for the joint
+ private: double dampCoef;
+
};
/** \} */
Modified: pkg/trunk/simulators/gazebo_plugin/src/Pr2_Actarray.cc
===================================================================
--- pkg/trunk/simulators/gazebo_plugin/src/Pr2_Actarray.cc 2008-07-10 19:14:58 UTC (rev 1415)
+++ pkg/trunk/simulators/gazebo_plugin/src/Pr2_Actarray.cc 2008-07-10 19:28:11 UTC (rev 1416)
@@ -160,14 +160,15 @@
break;
}
- this->myIface->data->actuators[i].saturationTorque = jNode->GetDouble("saturationTorque",0.0,1);
- this->myIface->data->actuators[i].pGain = jNode->GetDouble("pGain" ,0.0,1);
- this->myIface->data->actuators[i].iGain = jNode->GetDouble("iGain" ,0.0,1);
- this->myIface->data->actuators[i].dGain = jNode->GetDouble("dGain" ,0.0,1);
- this->myIface->data->actuators[i].iClamp = jNode->GetDouble("iClamp",0.0,1);
- //this->myIface->data->actuators[i].cmdPosition = 0.0;
- //this->myIface->data->actuators[i].cmdSpeed = 0.0;
- std::string tmpControlMode = jNode->GetString("controlMode","PD_CONTROL",0);
+ this->myIface->data->actuators[i].saturationTorque = jNode->GetDouble("saturationTorque",0.0,1);
+ this->myIface->data->actuators[i].pGain = jNode->GetDouble("pGain" ,0.0,1);
+ this->myIface->data->actuators[i].iGain = jNode->GetDouble("iGain" ,0.0,1);
+ this->myIface->data->actuators[i].dGain = jNode->GetDouble("dGain" ,0.0,1);
+ this->myIface->data->actuators[i].iClamp = jNode->GetDouble("iClamp",0.0,1);
+ //this->myIface->data->actuators[i].cmdPosition = 0.0;
+ //this->myIface->data->actuators[i].cmdSpeed = 0.0;
+ std::string tmpControlMode = jNode->GetString("controlMode","PD_CONTROL",0);
+ this->myIface->data->actuators[i].dampingCoefficient = jNode->GetDouble("explicitDampingCoefficient",0.0,0);
// init a new pid for this joint
this->pids[i] = new Pid();
@@ -221,68 +222,68 @@
#ifdef ADVAIT
void Pr2_Actarray::UpdateChild()
{
- HingeJoint *hjoint;
- double cmdPosition;
- double currentTime;
+ HingeJoint *hjoint;
+ double cmdPosition;
+ double currentTime;
- double curr_ang;
- double mass, length;
- double g;
- double gravity_torque;
- static double error, error_prev=-200;
- double kp,kd;
- double apply_torque;
+ double curr_ang;
+ double mass, length;
+ double g;
+ double gravity_torque;
+ static double error, error_prev=-200;
+ double kp,kd;
+ double apply_torque;
- this->myIface->Lock(1);
+ this->myIface->Lock(1);
- currentTime = Simulator::Instance()->GetSimTime();
- this->myIface->data->head.time = currentTime;
- this->myIface->data->actuators_count = this->num_joints;
+ currentTime = Simulator::Instance()->GetSimTime();
+ this->myIface->data->head.time = currentTime;
+ this->myIface->data->actuators_count = this->num_joints;
- //--------- loop through all the controllable dof's in this interface ----------
- for (int count = 0; count < this->num_joints; count++)
- {
- switch(this->joints[count]->GetType())
- {
- case Joint::HINGE:
- hjoint = dynamic_cast<HingeJoint*>(this->joints[count]);
- cmdPosition = this->myIface->data->actuators[count].cmdPosition;
- cmdPosition = DTOR(60.0);
- curr_ang = hjoint->GetAngle();
- mass = 5;
- length = 0.25;
- g = 9.8;
- gravity_torque = -mass*g*cos(curr_ang)*length;
- error = cmdPosition-curr_ang;
- if (error_prev == -200)
- error_prev = error;
+ //--------- loop through all the controllable dof's in this interface ----------
+ for (int count = 0; count < this->num_joints; count++)
+ {
+ switch(this->joints[count]->GetType())
+ {
+ case Joint::HINGE:
+ hjoint = dynamic_cast<HingeJoint*>(this->joints[count]);
+ cmdPosition = this->myIface->data->actuators[count].cmdPosition;
+ cmdPosition = DTOR(60.0);
+ curr_ang = hjoint->GetAngle();
+ mass = 5;
+ length = 0.25;
+ g = 9.8;
+ gravity_torque = -mass*g*cos(curr_ang)*length;
+ error = cmdPosition-curr_ang;
+ if (error_prev == -200)
+ error_prev = error;
- kp = 1.;
- kd = 50.0;
- printf("---------------\n");
- printf("error: %f\t error_prev: %f\n", RTOD(error), RTOD(error_prev));
- printf("kp term: %.3f\tkd term: %.3f\t", kp*error, kd*(error-error_prev));
- apply_torque = -gravity_torque + kp*error + kd*(error-error_prev);
- error_prev = error;
- printf("error: %.2f\tapplied torque:%f\n", RTOD(error), apply_torque);
- hjoint->SetTorque(apply_torque);
+ kp = 1.;
+ kd = 50.0;
+ printf("---------------\n");
+ printf("error: %f\t error_prev: %f\n", RTOD(error), RTOD(error_prev));
+ printf("kp term: %.3f\tkd term: %.3f\t", kp*error, kd*(error-error_prev));
+ apply_torque = -gravity_torque + kp*error + kd*(error-error_prev);
+ error_prev = error;
+ printf("error: %.2f\tapplied torque:%f\n", RTOD(error), apply_torque);
+ hjoint->SetTorque(apply_torque);
- this->myIface->data->actuators[count].actualPosition = hjoint->GetAngle();
- this->myIface->data->actuators[count].actualSpeed = hjoint->GetAngleRate();
- this->myIface->data->actuators[count].actualEffectorForce = 0.0; //TODO: use JointFeedback struct at some point
- break;
+ this->myIface->data->actuators[count].actualPosition = hjoint->GetAngle();
+ this->myIface->data->actuators[count].actualSpeed = hjoint->GetAngleRate();
+ this->myIface->data->actuators[count].actualEffectorForce = 0.0; //TODO: use JointFeedback struct at some point
+ break;
- case Joint::SLIDER:
- case Joint::HINGE2:
- case Joint::BALL:
- case Joint::UNIVERSAL:
- break;
- }
- }
+ case Joint::SLIDER:
+ case Joint::HINGE2:
+ case Joint::BALL:
+ case Joint::UNIVERSAL:
+ break;
+ }
+ }
- this->myIface->data->new_cmd = 0;
- this->myIface->Unlock();
- this->lastTime = currentTime;
+ this->myIface->data->new_cmd = 0;
+ this->myIface->Unlock();
+ this->lastTime = currentTime;
}
#else
@@ -328,8 +329,10 @@
double currentTime;
double currentCmd;
double currentRate;
-double currentAngle;
+ double currentAngle;
+ // TODO: EXPERIMENTAL: add explicit damping
+ double dampForce;
this->myIface->Lock(1);
@@ -404,15 +407,29 @@
switch(this->myIface->data->actuators[count].controlMode)
{
case PR2::TORQUE_CONTROL:
- printf("Hinge Torque Control\n");
- hjoint->SetTorque(this->myIface->data->actuators[count].cmdEffectorForce);
+ // TODO: EXPERIMENTAL: add explicit damping
+ currentRate = hjoint->GetAngleRate();
+ dampForce = - this->myIface->data->actuators[count].dampingCoefficient * currentRate;
+ dampForce = (dampForce > this->myIface->data->actuators[count].saturationTorque ) ? this->myIface->data->actuators[count].saturationTorque : dampForce;
+ dampForce = (dampForce < -this->myIface->data->actuators[count].saturationTorque ) ? -this->myIface->data->actuators[count].saturationTorque : dampForce;
+ printf("Damping f %f v %f\n",dampForce,currentRate);
+
+ // simply set torque
+ hjoint->SetTorque(this->myIface->data->actuators[count].cmdEffectorForce + dampForce);
//std::cout << count << " " << this->myIface->data->actuators[count].controlMode << std::endl;
break;
case PR2::PD_TORQUE_CONTROL :
+ // TODO: EXPERIMENTAL: add explicit damping
+ currentRate = hjoint->GetAngleRate();
+ dampForce = - this->myIface->data->actuators[count].dampingCoefficient * currentRate;
+ dampForce = (dampForce > this->myIface->data->actuators[count].saturationTorque ) ? this->myIface->data->actuators[count].saturationTorque : dampForce;
+ dampForce = (dampForce < -this->myIface->data->actuators[count].saturationTorque ) ? -this->myIface->data->actuators[count].saturationTorque : dampForce;
+ printf("Damping f %f v %f\n",dampForce,currentRate);
+
currentAngle = hjoint->GetAngle();
// No fancy controller, just pass the commanded torque/force in (we are not modeling the motors for now)
positionError = ModNPi2Pi(cmdPosition - currentAngle);
- speedError = cmdSpeed - hjoint->GetAngleRate();
+ speedError = cmdSpeed - currentRate;
currentCmd = this->pids[count]->UpdatePid(positionError + 0.0*speedError, currentTime-this->lastTime);
// if(count==PR2::ARM_R_SHOULDER_PITCH ) std::cout << "hinge err:" << positionError << " cmd: " << currentCmd << std::endl;
//Write out data
@@ -424,7 +441,7 @@
//currentCmd = (currentCmd > this->myIface->data->actuators[count].saturationTorque ) ? this->myIface->data->actuators[count].saturationTorque : currentCmd;
//currentCmd = (currentCmd < -this->myIface->data->actuators[count].saturationTorque ) ? -this->myIface->data->actuators[count].saturationTorque : currentCmd;
hjoint->SetParam( dParamFMax, 0);
- hjoint->SetTorque(currentCmd);
+ hjoint->SetTorque(currentCmd + dampForce);
break;
case PR2::SPEED_TORQUE_CONTROL :
@@ -450,8 +467,10 @@
//else if (cmdPosition < hjoint->GetLowStop())
// cmdPosition = hjoint->GetLowStop();
- positionError = ModNPi2Pi(cmdPosition - hjoint->GetAngle());
- speedError = cmdSpeed - hjoint->GetAngleRate();
+ currentAngle = hjoint->GetAngle();
+ currentRate = hjoint->GetAngleRate();
+ positionError = ModNPi2Pi(cmdPosition - currentAngle);
+ speedError = cmdSpeed - currentRate;
//std::cout << "hinge e:" << speedError << " + " << positionError << std::endl;
currentCmd = this->pids[count]->UpdatePid(positionError + 0.0*speedError, currentTime-this->lastTime);
hjoint->SetParam( dParamVel, currentCmd );
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <ehb...@us...> - 2008-07-10 23:01:04
|
Revision: 1430
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=1430&view=rev
Author: ehberger
Date: 2008-07-10 16:01:03 -0700 (Thu, 10 Jul 2008)
Log Message:
-----------
in-progress mechanism control infrastructure
Added Paths:
-----------
pkg/trunk/mechanism/
pkg/trunk/mechanism/Makefile
pkg/trunk/mechanism/include/
pkg/trunk/mechanism/manifest.xml
pkg/trunk/mechanism/src/
pkg/trunk/mechanism/src/Makefile
pkg/trunk/mechanism/src/hardware_interface.h
pkg/trunk/mechanism/src/joint.h
pkg/trunk/mechanism/src/link.h
pkg/trunk/mechanism/src/mechanism_controller.cpp
pkg/trunk/mechanism/src/mechanism_controller.h
pkg/trunk/mechanism/src/null_hardware.cpp
pkg/trunk/mechanism/src/null_hardware.h
pkg/trunk/mechanism/src/null_mechanism_controller.cpp
pkg/trunk/mechanism/src/null_mechanism_controller.h
pkg/trunk/mechanism/src/robot.h
pkg/trunk/mechanism/src/transmission.h
Added: pkg/trunk/mechanism/Makefile
===================================================================
--- pkg/trunk/mechanism/Makefile (rev 0)
+++ pkg/trunk/mechanism/Makefile 2008-07-10 23:01:03 UTC (rev 1430)
@@ -0,0 +1,3 @@
+SUBDIRS = src
+include $(shell rospack find mk)/recurse_subdirs.mk
+
Added: pkg/trunk/mechanism/manifest.xml
===================================================================
--- pkg/trunk/mechanism/manifest.xml (rev 0)
+++ pkg/trunk/mechanism/manifest.xml 2008-07-10 23:01:03 UTC (rev 1430)
@@ -0,0 +1,13 @@
+<package>
+<description brief="Mechanism control stub">
+</description>
+<author>Eric Berger be...@wi...</author>
+<license>BSD</license>
+<url>http://pr.willowgarage.com</url>
+<depend package="roscpp"/>
+<depend package="std_msgs"/>
+<export>
+ <cpp cflags="-I${prefix}/include"/>
+</export>
+</package>
+
Added: pkg/trunk/mechanism/src/Makefile
===================================================================
--- pkg/trunk/mechanism/src/Makefile (rev 0)
+++ pkg/trunk/mechanism/src/Makefile 2008-07-10 23:01:03 UTC (rev 1430)
@@ -0,0 +1,7 @@
+SRC = null_hardware.cpp null_mechanism_controller.cpp
+OUT = null_hardware
+PKG = mechanism
+
+include $(shell rospack find mk)/node.mk
+
+
Added: pkg/trunk/mechanism/src/hardware_interface.h
===================================================================
--- pkg/trunk/mechanism/src/hardware_interface.h (rev 0)
+++ pkg/trunk/mechanism/src/hardware_interface.h 2008-07-10 23:01:03 UTC (rev 1430)
@@ -0,0 +1,87 @@
+
+///////////////////////////////////////////////////////////////////////////////
+// Copyright (C) 2008, Eric Berger
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+// notice, this list of conditions and the following disclaimer in the
+// documentation and/or other materials provided with the distribution.
+// * Neither the name of Stanford University nor the names of its
+// contributors may be used to endorse or promote products derived from
+// this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//////////////////////////////////////////////////////////////////////////////
+
+#ifndef HARDWARE_INTERFACE_H
+#define HARDWARE_INTERFACE_H
+
+class ActuatorState{
+ public:
+ ActuatorState(){
+ lastCalibrationHighTransition = 0;
+ lastCalibrationLowTransition = 0;
+ isEnabled = 0;
+ runStopHit = 0;
+ lastRequestedCurrent = 0;
+ lastCommandedCurrent = 0;
+ lastMeasuredCurrent = 0;
+ numEncoderErrors = 0;
+ }
+ int encoderCount;
+ int timestamp;
+ double encoderVelocity;
+ bool calibrationReading;
+ int lastCalibrationHighTransition;
+ int lastCalibrationLowTransition;
+ bool isEnabled;
+ bool runStopHit;
+
+ double lastRequestedCurrent;
+ double lastCommandedCurrent;
+ double lastMeasuredCurrent;
+
+ int numEncoderErrors;
+ int numCommunicationErrors;
+};
+
+class ActuatorCommand{
+ public:
+ ActuatorCommand(){
+ enable = 0;
+ current = 0;
+ }
+ bool enable;
+ double current;
+};
+
+class Actuator{
+ public:
+ ActuatorState state;
+ ActuatorCommand command;
+};
+
+class HardwareInterface{
+ public:
+ HardwareInterface(int numActuators){
+ actuator = new Actuator[numActuators];
+ this->numActuators = numActuators;
+ }
+ Actuator *actuator;
+ int numActuators;
+};
+
+#endif
Added: pkg/trunk/mechanism/src/joint.h
===================================================================
--- pkg/trunk/mechanism/src/joint.h (rev 0)
+++ pkg/trunk/mechanism/src/joint.h 2008-07-10 23:01:03 UTC (rev 1430)
@@ -0,0 +1,47 @@
+
+///////////////////////////////////////////////////////////////////////////////
+// Copyright (C) 2008, Eric Berger
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+// notice, this list of conditions and the following disclaimer in the
+// documentation and/or other materials provided with the distribution.
+// * Neither the name of Stanford University nor the names of its
+// contributors may be used to endorse or promote products derived from
+// this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//////////////////////////////////////////////////////////////////////////////
+
+class Joint{
+ char *name;
+ int type;
+
+ //Update every cycle from input data
+ bool initialized;
+ double position;
+ double velocity;
+ double appliedEffort;
+
+ //Written every cycle out to motor boards
+ double commandedEffort;
+
+ //Never changes
+ double jointLimitMin;
+ double jointLimitMax;
+ double effortLimit;
+ double velocityLimit;
+};
Added: pkg/trunk/mechanism/src/link.h
===================================================================
--- pkg/trunk/mechanism/src/link.h (rev 0)
+++ pkg/trunk/mechanism/src/link.h 2008-07-10 23:01:03 UTC (rev 1430)
@@ -0,0 +1,33 @@
+
+///////////////////////////////////////////////////////////////////////////////
+// Copyright (C) 2008, Eric Berger
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+// notice, this list of conditions and the following disclaimer in the
+// documentation and/or other materials provided with the distribution.
+// * Neither the name of Stanford University nor the names of its
+// contributors may be used to endorse or promote products derived from
+// this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//////////////////////////////////////////////////////////////////////////////
+
+class Link{
+ char *name;
+ //Pointer to KDL link
+ Joint *joint;
+};
Added: pkg/trunk/mechanism/src/mechanism_controller.cpp
===================================================================
--- pkg/trunk/mechanism/src/mechanism_controller.cpp (rev 0)
+++ pkg/trunk/mechanism/src/mechanism_controller.cpp 2008-07-10 23:01:03 UTC (rev 1430)
@@ -0,0 +1,54 @@
+
+///////////////////////////////////////////////////////////////////////////////
+// Copyright (C) 2008, Eric Berger
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+// notice, this list of conditions and the following disclaimer in the
+// documentation and/or other materials provided with the distribution.
+// * Neither the name of Stanford University nor the names of its
+// contributors may be used to endorse or promote products derived from
+// this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//////////////////////////////////////////////////////////////////////////////
+
+MechanismController::MechanismController(HardwareInterface *hw){
+ this->hw = hw;
+ //Needs to learn pr2.xml filename or get it from param server
+ //Create robot model
+ //Set up transmissions, joints, and links
+
+ //Set up ROS
+ //Subscribe to "n
+ //Advertise "new controller" service
+}
+
+void MechanismController::update(){
+ //Clear actuator commands
+ for(int i = 0; i < numTransmissions; i++){
+ r->transmission[i].propagatePosition();
+ }
+ //update KDL model with new joint position/velocities
+ //
+ //update controllers
+ for(int i = 0; i < numJoints; i++){
+ r->joint[i].enforceLimits();
+ }
+ for(int i = 0; i < numTransmissions; i++){
+ r->transmission[i].propagateEffort();
+ }
+}
Added: pkg/trunk/mechanism/src/mechanism_controller.h
===================================================================
--- pkg/trunk/mechanism/src/mechanism_controller.h (rev 0)
+++ pkg/trunk/mechanism/src/mechanism_controller.h 2008-07-10 23:01:03 UTC (rev 1430)
@@ -0,0 +1,39 @@
+
+///////////////////////////////////////////////////////////////////////////////
+// Copyright (C) 2008, Eric Berger
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+// notice, this list of conditions and the following disclaimer in the
+// documentation and/or other materials provided with the distribution.
+// * Neither the name of Stanford University nor the names of its
+// contributors may be used to endorse or promote products derived from
+// this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//////////////////////////////////////////////////////////////////////////////
+#ifndef MECHANISM_CONTROLL_H
+#define MECHANISM_CONTROLLER_H
+
+#include "hardware_interface.h"
+
+//Base requirements for a piece of code that will control a full mechanism
+class MechanismController{
+ public:
+ void update(); //Must be realtime safe
+};
+
+#endif
Added: pkg/trunk/mechanism/src/null_hardware.cpp
===================================================================
--- pkg/trunk/mechanism/src/null_hardware.cpp (rev 0)
+++ pkg/trunk/mechanism/src/null_hardware.cpp 2008-07-10 23:01:03 UTC (rev 1430)
@@ -0,0 +1,68 @@
+
+///////////////////////////////////////////////////////////////////////////////
+// Copyright (C) 2008, Eric Berger
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+// notice, this list of conditions and the following disclaimer in the
+// documentation and/or other materials provided with the distribution.
+// * Neither the name of Stanford University nor the names of its
+// contributors may be used to endorse or promote products derived from
+// this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//////////////////////////////////////////////////////////////////////////////
+
+//Null hardware implementation
+//Creates empty robot and runs it with null controller
+
+#include "null_hardware.h"
+#include "null_mechanism_controller.h"
+
+NullHardware::NullHardware(){
+ //Read actuators.xml and initialize hardware
+ int numActuators = 10;
+ HardwareInterface *hw = new HardwareInterface(numActuators);
+
+ controller = new NullMechanismController(hw);
+ for(int i = 0; i < 100; i++){
+ //Read in data from hardware
+ for(int i = 0; i < hw->numActuators; i++){
+ updateState(&hw->actuator[i].state);
+ }
+ //Fill in HardwareInterface with all new status
+ controller->update();
+ //Send HardwareInterface commands out to motors
+ for(int i = 0; i < hw->numActuators; i++){
+ readCommand(&hw->actuator[i].command);
+ }
+ //Sleep(1)
+ }
+}
+
+void NullHardware::updateState(ActuatorState *state){
+ state->timestamp++;
+ state->encoderCount = 100;
+}
+
+void NullHardware::readCommand(ActuatorCommand *command){
+ double current = command->current;
+ //writeMotor(current);
+}
+
+int main(int argc, char *argv[]){
+ NullHardware *h = new NullHardware();
+}
Added: pkg/trunk/mechanism/src/null_hardware.h
===================================================================
--- pkg/trunk/mechanism/src/null_hardware.h (rev 0)
+++ pkg/trunk/mechanism/src/null_hardware.h 2008-07-10 23:01:03 UTC (rev 1430)
@@ -0,0 +1,44 @@
+
+///////////////////////////////////////////////////////////////////////////////
+// Copyright (C) 2008, Eric Berger
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+// notice, this list of conditions and the following disclaimer in the
+// documentation and/or other materials provided with the distribution.
+// * Neither the name of Stanford University nor the names of its
+// contributors may be used to endorse or promote products derived from
+// this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//////////////////////////////////////////////////////////////////////////////
+
+#ifndef NULL_HARDWARE_H
+#define NULL_HARDWARE_H
+
+#include "hardware_interface.h"
+#include "null_mechanism_controller.h"
+
+class NullHardware{
+ public:
+ NullHardware(void);
+ private:
+ void updateState(ActuatorState *state);
+ void readCommand(ActuatorCommand *command);
+ NullMechanismController *controller;
+};
+
+#endif
Added: pkg/trunk/mechanism/src/null_mechanism_controller.cpp
===================================================================
--- pkg/trunk/mechanism/src/null_mechanism_controller.cpp (rev 0)
+++ pkg/trunk/mechanism/src/null_mechanism_controller.cpp 2008-07-10 23:01:03 UTC (rev 1430)
@@ -0,0 +1,40 @@
+
+///////////////////////////////////////////////////////////////////////////////
+// Copyright (C) 2008, Eric Berger
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+// notice, this list of conditions and the following disclaimer in the
+// documentation and/or other materials provided with the distribution.
+// * Neither the name of Stanford University nor the names of its
+// contributors may be used to endorse or promote products derived from
+// this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//////////////////////////////////////////////////////////////////////////////
+
+#include "null_mechanism_controller.h"
+#include "stdio.h"
+
+
+NullMechanismController::NullMechanismController(HardwareInterface *hw){
+ this->hw = hw;
+}
+
+void NullMechanismController::update(){
+ printf("Running update\n");
+ printf("reading from %d actuators.\n", hw->numActuators);
+}
Added: pkg/trunk/mechanism/src/null_mechanism_controller.h
===================================================================
--- pkg/trunk/mechanism/src/null_mechanism_controller.h (rev 0)
+++ pkg/trunk/mechanism/src/null_mechanism_controller.h 2008-07-10 23:01:03 UTC (rev 1430)
@@ -0,0 +1,42 @@
+
+///////////////////////////////////////////////////////////////////////////////
+// Copyright (C) 2008, Eric Berger
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+// notice, this list of conditions and the following disclaimer in the
+// documentation and/or other materials provided with the distribution.
+// * Neither the name of Stanford University nor the names of its
+// contributors may be used to endorse or promote products derived from
+// this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//////////////////////////////////////////////////////////////////////////////
+
+#ifndef NULL_MECHANISM_CONTROLLER_H
+#define NULL_MECHANISM_CONTROLLER_H
+
+#include "hardware_interface.h"
+
+class NullMechanismController{
+ public:
+ NullMechanismController(HardwareInterface *hw);
+ void update();
+
+ HardwareInterface *hw;
+};
+
+#endif
Added: pkg/trunk/mechanism/src/robot.h
===================================================================
--- pkg/trunk/mechanism/src/robot.h (rev 0)
+++ pkg/trunk/mechanism/src/robot.h 2008-07-10 23:01:03 UTC (rev 1430)
@@ -0,0 +1,42 @@
+
+///////////////////////////////////////////////////////////////////////////////
+// Copyright (C) 2008, Eric Berger
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+// notice, this list of conditions and the following disclaimer in the
+// documentation and/or other materials provided with the distribution.
+// * Neither the name of Stanford University nor the names of its
+// contributors may be used to endorse or promote products derived from
+// this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//////////////////////////////////////////////////////////////////////////////
+
+//The robot model is populated by the control code infrastructure and used by all the controllers to read mechanism state and command mechanism motion.
+
+class Robot{
+ public:
+ Robot(char *namespace);
+ ~Robot();
+ char *name;
+ Link *link;
+ int numLinks;
+ Joint *joint;
+ int numJoints;
+ Transmission *transmission;
+ int numTransmissions;
+}
Added: pkg/trunk/mechanism/src/transmission.h
===================================================================
--- pkg/trunk/mechanism/src/transmission.h (rev 0)
+++ pkg/trunk/mechanism/src/transmission.h 2008-07-10 23:01:03 UTC (rev 1430)
@@ -0,0 +1,52 @@
+
+///////////////////////////////////////////////////////////////////////////////
+// Copyright (C) 2008, Eric Berger
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+// notice, this list of conditions and the following disclaimer in the
+// documentation and/or other materials provided with the distribution.
+// * Neither the name of Stanford University nor the names of its
+// contributors may be used to endorse or promote products derived from
+// this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//////////////////////////////////////////////////////////////////////////////
+
+class Transmission(){
+ public:
+ void propagatePositions(); //Use encoder data to fill out joint position and velocities
+ void propagateEfforts(); //Use commanded joint efforts to fill out commanded motor currents
+};
+
+class SimpleTransmission : public Transmision{
+ public:
+ Actuator *actuator;
+ Joint *joint;
+ double mechanicalReduction;
+ double motorTorqueConstant;
+};
+
+class CoupledTransmission : public Transmission{
+ public:
+
+};...
[truncated message content] |
|
From: <hsu...@us...> - 2008-07-10 23:37:10
|
Revision: 1443
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=1443&view=rev
Author: hsujohnhsu
Date: 2008-07-10 16:37:08 -0700 (Thu, 10 Jul 2008)
Log Message:
-----------
use UpdateHW call in libpr2HW to do the interfacing for actuator arrays.
Not done for sensors and not done for groundtruth positions.
using a temporary JointData Struct in pr2Core.h for now.
pr2MechCon is calling UpdateHW() in the "RT" loop.
Modified Paths:
--------------
pkg/trunk/3rdparty/gazebo/Makefile
pkg/trunk/demos/2dnav-gazebo/world/robot_torque.world
pkg/trunk/drivers/robot/pr2/libpr2HW/include/libpr2HW/pr2HW.h
pkg/trunk/drivers/robot/pr2/libpr2HW/src/pr2HW.cpp
pkg/trunk/drivers/robot/pr2/pr2Core/include/pr2Core/pr2Core.h
pkg/trunk/simulators/pr2MechCon/src/pr2MechCon.cpp
Modified: pkg/trunk/3rdparty/gazebo/Makefile
===================================================================
--- pkg/trunk/3rdparty/gazebo/Makefile 2008-07-10 23:35:02 UTC (rev 1442)
+++ pkg/trunk/3rdparty/gazebo/Makefile 2008-07-10 23:37:08 UTC (rev 1443)
@@ -14,7 +14,7 @@
@if [ ! -d gazebo-svn ]; then svn co $(REVISION) https://playerstage.svn.sf.net/svnroot/playerstage/code/gazebo/trunk gazebo-svn; fi
-cd gazebo-svn && svn up $(REVISION)
-svn diff gazebo-svn > tmp.diff
- #-cd gazebo-svn && svn revert -R *
+ -cd gazebo-svn && svn revert -R *
-patch -N -p0 < gazebo_patch.diff
gazebo: gazebo-checkout
Modified: pkg/trunk/demos/2dnav-gazebo/world/robot_torque.world
===================================================================
--- pkg/trunk/demos/2dnav-gazebo/world/robot_torque.world 2008-07-10 23:35:02 UTC (rev 1442)
+++ pkg/trunk/demos/2dnav-gazebo/world/robot_torque.world 2008-07-10 23:37:08 UTC (rev 1443)
@@ -538,7 +538,7 @@
<dGain>0</dGain>
<iGain>500</iGain>
<iClamp>500</iClamp>
- <explicitDampingCoefficient>10.0</explicitDampingCoefficient>
+ <explicitDampingCoefficient>50.0</explicitDampingCoefficient>
<controlMode>PD_TORQUE_CONTROL</controlMode>
</joint>
<joint name="upperarm_pitch_joint_left">
@@ -552,11 +552,11 @@
</joint>
<joint name="upperarm_roll_joint_left">
<saturationTorque>1000</saturationTorque>
- <pGain>100</pGain>
- <dGain>0</dGain>
- <iGain>10</iGain>
- <iClamp>10</iClamp>
- <explicitDampingCoefficient>2.0</explicitDampingCoefficient>
+ <pGain>1000</pGain>
+ <dGain>10</dGain>
+ <iGain>100</iGain>
+ <iClamp>100</iClamp>
+ <explicitDampingCoefficient>10.0</explicitDampingCoefficient>
<controlMode>PD_TORQUE_CONTROL</controlMode>
</joint>
<joint name="elbow_pitch_joint_left">
@@ -602,7 +602,7 @@
<dGain>0</dGain>
<iGain>500</iGain>
<iClamp>500</iClamp>
- <explicitDampingCoefficient>10.0</explicitDampingCoefficient>
+ <explicitDampingCoefficient>50.0</explicitDampingCoefficient>
<controlMode>PD_TORQUE_CONTROL</controlMode>
</joint>
<joint name="upperarm_pitch_joint_right">
Modified: pkg/trunk/drivers/robot/pr2/libpr2HW/include/libpr2HW/pr2HW.h
===================================================================
--- pkg/trunk/drivers/robot/pr2/libpr2HW/include/libpr2HW/pr2HW.h 2008-07-10 23:35:02 UTC (rev 1442)
+++ pkg/trunk/drivers/robot/pr2/libpr2HW/include/libpr2HW/pr2HW.h 2008-07-10 23:37:08 UTC (rev 1443)
@@ -346,6 +346,16 @@
*/
public: NEWMAT::Matrix GetWristPoseGroundTruth(PR2_MODEL_ID id);
+ /*!
+ * \brief Joint Data for Arm
+ *
+ * FIXME: all this stuff will be in the robot data structure, constructed by XML
+ *
+ */
+ public: PR2::JointData jointData[MAX_JOINT_IDS];
+
+
+
};
}
Modified: pkg/trunk/drivers/robot/pr2/libpr2HW/src/pr2HW.cpp
===================================================================
--- pkg/trunk/drivers/robot/pr2/libpr2HW/src/pr2HW.cpp 2008-07-10 23:35:02 UTC (rev 1442)
+++ pkg/trunk/drivers/robot/pr2/libpr2HW/src/pr2HW.cpp 2008-07-10 23:37:08 UTC (rev 1443)
@@ -246,6 +246,71 @@
<< e << "\n";
pr2BaseIface = NULL;
}
+
+
+
+
+
+ std::cout << "initial HW reads\n" << std::endl;
+ // send commands to hardware
+ for (int id = PR2::CASTER_FL_STEER; id < PR2::HEAD_PTZ_R_TILT; id++)
+ {
+ if(IsHead((PR2::PR2_JOINT_ID)id))
+ {
+ pr2HeadIface->Lock(1);
+ this->jointData[id].cmdEnableMotor = pr2HeadIface->data->actuators[id-JointStart[HEAD]].cmdEnableMotor ;
+ this->jointData[id].controlMode = pr2HeadIface->data->actuators[id-JointStart[HEAD]].controlMode ;
+ this->jointData[id].pGain = pr2HeadIface->data->actuators[id-JointStart[HEAD]].pGain ;
+ this->jointData[id].iGain = pr2HeadIface->data->actuators[id-JointStart[HEAD]].iGain ;
+ this->jointData[id].dGain = pr2HeadIface->data->actuators[id-JointStart[HEAD]].dGain ;
+ this->jointData[id].cmdPosition = pr2HeadIface->data->actuators[id-JointStart[HEAD]].cmdPosition ;
+ this->jointData[id].cmdSpeed = pr2HeadIface->data->actuators[id-JointStart[HEAD]].cmdSpeed ;
+ this->jointData[id].cmdEffectorForce = pr2HeadIface->data->actuators[id-JointStart[HEAD]].cmdEffectorForce ;
+ pr2HeadIface->Unlock();
+ }
+ else if(IsGripperLeft((PR2::PR2_JOINT_ID)id))
+ {
+ pr2GripperLeftIface->Lock(1);
+ this->jointData[id].cmdEnableMotor = pr2GripperLeftIface->data->cmdEnableMotor ;
+ this->jointData[id].controlMode = pr2GripperLeftIface->data->controlMode ;
+ this->jointData[id].pGain = pr2GripperLeftIface->data->pGain ;
+ this->jointData[id].iGain = pr2GripperLeftIface->data->iGain ;
+ this->jointData[id].dGain = pr2GripperLeftIface->data->dGain ;
+ this->jointData[id].cmdGap = pr2GripperLeftIface->data->cmdGap ;
+ this->jointData[id].cmdEffectorForce = pr2GripperLeftIface->data->cmdForce ;
+ this->jointData[id].cmdSpeed = pr2GripperLeftIface->data->cmdPositionRate ;
+ pr2GripperLeftIface->Unlock();
+ }
+ else if(IsGripperRight((PR2::PR2_JOINT_ID)id))
+ {
+ pr2GripperRightIface->Lock(1);
+ this->jointData[id].cmdEnableMotor = pr2GripperRightIface->data->cmdEnableMotor ;
+ this->jointData[id].controlMode = pr2GripperRightIface->data->controlMode ;
+ this->jointData[id].pGain = pr2GripperRightIface->data->pGain ;
+ this->jointData[id].iGain = pr2GripperRightIface->data->iGain ;
+ this->jointData[id].dGain = pr2GripperRightIface->data->dGain ;
+ this->jointData[id].cmdGap = pr2GripperRightIface->data->cmdGap ;
+ this->jointData[id].cmdEffectorForce = pr2GripperRightIface->data->cmdForce ;
+ this->jointData[id].cmdSpeed = pr2GripperRightIface->data->cmdPositionRate ;
+ pr2GripperRightIface->Unlock();
+ }
+ else // corresponds to Pr2_Actarray from CASTER to ARM_R_GRIPPER
+ {
+ pr2Iface->Lock(1);
+ this->jointData[id].cmdEnableMotor = pr2Iface->data->actuators[id].cmdEnableMotor ;
+ this->jointData[id].controlMode = pr2Iface->data->actuators[id].controlMode ;
+ this->jointData[id].pGain = pr2Iface->data->actuators[id].pGain ;
+ this->jointData[id].iGain = pr2Iface->data->actuators[id].iGain ;
+ this->jointData[id].dGain = pr2Iface->data->actuators[id].dGain ;
+ this->jointData[id].cmdPosition = pr2Iface->data->actuators[id].cmdPosition ;
+ this->jointData[id].cmdSpeed = pr2Iface->data->actuators[id].cmdSpeed ;
+ this->jointData[id].cmdEffectorForce = pr2Iface->data->actuators[id].cmdEffectorForce ;
+ pr2Iface->Unlock();
+ }
+ }
+
+
+
return PR2_ALL_OK;
};
@@ -263,288 +328,72 @@
PR2_ERROR_CODE PR2HW::EnableModel(PR2_MODEL_ID id)
{
- if(IsHead(id))
- {
- pr2HeadIface->Lock(1);
- for(int ii = JointStart[id]; ii <= JointEnd[id]; ii++)
- // subtrace JointStart[HEAD] since HEAD controllers is a new actarray.
- pr2HeadIface->data->actuators[ii-JointStart[HEAD]].cmdEnableMotor = 1;
- pr2Iface->Unlock();
- }
- else if(IsGripperLeft(id))
- {
- // there is only one... for now
- pr2GripperLeftIface->Lock(1);
- pr2GripperLeftIface->data->cmdEnableMotor = 1;
- pr2GripperLeftIface->Unlock();
- }
- else if(IsGripperRight(id))
- {
- pr2GripperRightIface->Lock(1);
- pr2GripperRightIface->data->cmdEnableMotor = 1;
- pr2GripperRightIface->Unlock();
- }
- else
- {
- pr2Iface->Lock(1);
- for(int ii = JointStart[id]; ii <= JointEnd[id]; ii++)
- pr2Iface->data->actuators[ii].cmdEnableMotor = 1;
- pr2Iface->Unlock();
- }
+ for(int ii = JointStart[id]; ii <= JointEnd[id]; ii++)
+ this->jointData[ii].cmdEnableMotor = 1;
return PR2_ALL_OK;
};
PR2_ERROR_CODE PR2HW::DisableModel(PR2_MODEL_ID id)
{
- if(IsHead(id))
- {
- pr2HeadIface->Lock(1);
- for(int ii = JointStart[id]; ii <= JointEnd[id]; ii++)
- pr2HeadIface->data->actuators[ii-JointStart[HEAD]].cmdEnableMotor = 0;
- pr2HeadIface->Unlock();
- }
- else if(IsGripperLeft(id))
- {
- pr2GripperLeftIface->Lock(1);
- pr2GripperLeftIface->data->cmdEnableMotor = 0;
- pr2GripperLeftIface->Unlock();
- }
- else if(IsGripperRight(id))
- {
- pr2GripperRightIface->Lock(1);
- pr2GripperRightIface->data->cmdEnableMotor = 0;
- pr2GripperRightIface->Unlock();
- }
- else
- {
- pr2Iface->Lock(1);
- for(int ii = JointStart[id]; ii <= JointEnd[id]; ii++)
- pr2Iface->data->actuators[ii].cmdEnableMotor = 0;
- pr2Iface->Unlock();
- }
+ for(int ii = JointStart[id]; ii <= JointEnd[id]; ii++)
+ this->jointData[ii].cmdEnableMotor = 0;
return PR2_ALL_OK;
};
PR2_ERROR_CODE PR2HW::IsEnabledModel(PR2_MODEL_ID id, int *enabled)
{
int isEnabled = 1;
- if(IsHead(id))
- {
- pr2HeadIface->Lock(1);
- for(int ii = JointStart[id]; ii <= JointEnd[id]; ii++)
- isEnabled = pr2HeadIface->data->actuators[id-JointStart[HEAD]].cmdEnableMotor && isEnabled;
- pr2HeadIface->Unlock();
- }
- else if(IsGripperLeft(id))
- {
- pr2GripperLeftIface->Lock(1);
- isEnabled = pr2GripperLeftIface->data->cmdEnableMotor && isEnabled;
- pr2GripperLeftIface->Unlock();
- }
- else if(IsGripperRight(id))
- {
- pr2GripperRightIface->Lock(1);
- isEnabled = pr2GripperRightIface->data->cmdEnableMotor && isEnabled;
- pr2GripperRightIface->Unlock();
- }
- else
- {
- pr2Iface->Lock(1);
- for(int ii = JointStart[id]; ii <= JointEnd[id]; ii++)
- isEnabled = pr2Iface->data->actuators[id].cmdEnableMotor && isEnabled;
- pr2Iface->Unlock();
- }
+ for(int ii = JointStart[id]; ii <= JointEnd[id]; ii++)
+ isEnabled = this->jointData[ii].cmdEnableMotor && isEnabled;
*enabled = isEnabled;
return PR2_ALL_OK;
};
PR2_ERROR_CODE PR2HW::SetJointControlMode(PR2_JOINT_ID id, PR2_JOINT_CONTROL_MODE mode)
{
- if(IsHead(id))
- {
- pr2HeadIface->Lock(1);
- pr2HeadIface->data->actuators[id-JointStart[HEAD]].controlMode = mode;
- pr2HeadIface->Unlock();
- }
- else if(IsGripperLeft(id))
- {
- pr2GripperLeftIface->Lock(1);
- pr2GripperLeftIface->data->controlMode = mode;
- pr2GripperLeftIface->Unlock();
- }
- else if(IsGripperRight(id))
- {
- pr2GripperRightIface->Lock(1);
- pr2GripperRightIface->data->controlMode = mode;
- pr2GripperRightIface->Unlock();
- }
- else
- {
- pr2Iface->Lock(1);
- pr2Iface->data->actuators[id].controlMode = mode;
- pr2Iface->Unlock();
- }
+ this->jointData[id].controlMode = mode;
return PR2_ALL_OK;
};
PR2_ERROR_CODE PR2HW::GetJointControlMode(PR2_JOINT_ID id, PR2_JOINT_CONTROL_MODE *mode)
{
- if(IsHead(id))
- {
- pr2HeadIface->Lock(1);
- *mode = (PR2_JOINT_CONTROL_MODE) pr2HeadIface->data->actuators[id-JointStart[HEAD]].controlMode;
- pr2HeadIface->Unlock();
- }
- else if(IsGripperLeft(id))
- {
- pr2GripperLeftIface->Lock(1);
- *mode = (PR2_JOINT_CONTROL_MODE) pr2GripperLeftIface->data->controlMode;
- pr2GripperLeftIface->Unlock();
- }
- else if(IsGripperRight(id))
- {
- pr2GripperRightIface->Lock(1);
- *mode = (PR2_JOINT_CONTROL_MODE) pr2GripperRightIface->data->controlMode;
- pr2GripperRightIface->Unlock();
- }
- else
- {
- pr2Iface->Lock(1);
- *mode = (PR2_JOINT_CONTROL_MODE) pr2Iface->data->actuators[id].controlMode;
- pr2Iface->Unlock();
- }
+ *mode = (PR2::PR2_JOINT_CONTROL_MODE)(this->jointData[id].controlMode);
return PR2_ALL_OK;
};
PR2_ERROR_CODE PR2HW::SetJointGains(PR2_JOINT_ID id, double pGain, double iGain, double dGain)
{
- if(IsHead(id))
- {
- pr2HeadIface->Lock(1);
- pr2HeadIface->data->actuators[id-JointStart[HEAD]].pGain = pGain;
- pr2HeadIface->data->actuators[id-JointStart[HEAD]].iGain = iGain;
- pr2HeadIface->data->actuators[id-JointStart[HEAD]].dGain = dGain;
- pr2HeadIface->Unlock();
- }
- else
- {
- pr2Iface->Lock(1);
- pr2Iface->data->actuators[id].pGain = pGain;
- pr2Iface->data->actuators[id].iGain = iGain;
- pr2Iface->data->actuators[id].dGain = dGain;
- pr2Iface->Unlock();
- }
+ this->jointData[id-JointStart[HEAD]].pGain = pGain;
+ this->jointData[id-JointStart[HEAD]].iGain = iGain;
+ this->jointData[id-JointStart[HEAD]].dGain = dGain;
return PR2_ALL_OK;
};
PR2_ERROR_CODE PR2HW::GetJointGains(PR2_JOINT_ID id, double *pGain, double *iGain, double *dGain)
{
- if(IsHead(id))
- {
- pr2HeadIface->Lock(1);
- *pGain = pr2HeadIface->data->actuators[id-JointStart[HEAD]].pGain;
- *iGain = pr2HeadIface->data->actuators[id-JointStart[HEAD]].iGain;
- *dGain = pr2HeadIface->data->actuators[id-JointStart[HEAD]].dGain;
- pr2HeadIface->Unlock();
- }
- else
- {
- pr2Iface->Lock(1);
- *pGain = pr2Iface->data->actuators[id].pGain;
- *iGain = pr2Iface->data->actuators[id].iGain;
- *dGain = pr2Iface->data->actuators[id].dGain;
- pr2Iface->Unlock();
- }
+ *pGain = this->jointData[id].pGain;
+ *iGain = this->jointData[id].iGain;
+ *dGain = this->jointData[id].dGain;
return PR2_ALL_OK;
-
};
PR2_ERROR_CODE PR2HW::EnableJoint(PR2_JOINT_ID id)
{
- if (IsHead(id))
- {
- pr2HeadIface->Lock(1);
- pr2HeadIface->data->actuators[id-JointStart[HEAD]].cmdEnableMotor = 1;
- pr2HeadIface->Unlock();
- }
- else if (IsGripperLeft(id))
- {
- pr2GripperLeftIface ->Lock(1);
- pr2GripperLeftIface ->data->cmdEnableMotor = 1;
- pr2GripperLeftIface ->Unlock();
- }
- else if (IsGripperRight(id))
- {
- pr2GripperRightIface->Lock(1);
- pr2GripperRightIface->data->cmdEnableMotor = 1;
- pr2GripperRightIface->Unlock();
- }
- else
- {
- pr2Iface->Lock(1);
- pr2Iface->data->actuators[id].cmdEnableMotor = 1;
- pr2Iface->Unlock();
- }
+ this->jointData[id].cmdEnableMotor = 1;
return PR2_ALL_OK;
};
PR2_ERROR_CODE PR2HW::DisableJoint(PR2_JOINT_ID id)
{
- if (IsHead(id))
- {
- pr2HeadIface->Lock(1);
- pr2HeadIface->data->actuators[id-JointStart[HEAD]].cmdEnableMotor = 0;
- pr2HeadIface->Unlock();
- }
- else if (IsGripperLeft(id))
- {
- pr2GripperLeftIface ->Lock(1);
- pr2GripperLeftIface ->data->cmdEnableMotor = 0;
- pr2GripperLeftIface ->Unlock();
- }
- else if (IsGripperRight(id))
- {
- pr2GripperRightIface->Lock(1);
- pr2GripperRightIface->data->cmdEnableMotor = 0;
- pr2GripperRightIface->Unlock();
- }
- else
- {
- pr2Iface->Lock(1);
- pr2Iface->data->actuators[id].cmdEnableMotor = 0;
- pr2Iface->Unlock();
- }
+ this->jointData[id].cmdEnableMotor = 0;
return PR2_ALL_OK;
};
PR2_ERROR_CODE PR2HW::IsEnabledJoint(PR2_JOINT_ID id, int *enabled)
{
- if (IsHead(id))
- {
- pr2HeadIface->Lock(1);
- *enabled = pr2HeadIface->data->actuators[id-JointStart[HEAD]].cmdEnableMotor;
- pr2HeadIface->Unlock();
- }
- else if (IsGripperLeft(id))
- {
- pr2GripperLeftIface->Lock(1);
- *enabled = pr2GripperLeftIface->data->cmdEnableMotor;
- pr2GripperLeftIface->Unlock();
- }
- else if (IsGripperRight(id))
- {
- pr2GripperRightIface->Lock(1);
- *enabled = pr2GripperRightIface->data->cmdEnableMotor;
- pr2GripperRightIface->Unlock();
- }
- else
- {
- pr2Iface->Lock(1);
- *enabled = pr2Iface->data->actuators[id].cmdEnableMotor;
- pr2Iface->Unlock();
- }
+ *enabled = this->jointData[id].cmdEnableMotor;
return PR2_ALL_OK;
};
@@ -562,179 +411,68 @@
PR2_ERROR_CODE PR2HW::SetJointServoCmd(PR2_JOINT_ID id, double jointPosition, double jointSpeed)
{
- if(IsHead(id))
- {
- pr2HeadIface->Lock(1);
- pr2HeadIface->data->actuators[id-JointStart[HEAD]].cmdPosition = jointPosition;
- pr2HeadIface->data->actuators[id-JointStart[HEAD]].cmdSpeed = jointSpeed;
- pr2HeadIface->Unlock();
- }
- else
- {
- pr2Iface->Lock(1);
- pr2Iface->data->actuators[id].cmdPosition = jointPosition;
- pr2Iface->data->actuators[id].cmdSpeed = jointSpeed;
- pr2Iface->Unlock();
- }
+ this->jointData[id].cmdPosition = jointPosition;
+ this->jointData[id].cmdSpeed = jointSpeed;
return PR2_ALL_OK;
};
PR2_ERROR_CODE PR2HW::GetJointServoCmd(PR2_JOINT_ID id, double *jointPosition, double *jointSpeed)
{
- if(IsHead(id))
- {
- pr2HeadIface->Lock(1);
- *jointPosition = pr2HeadIface->data->actuators[id-JointStart[HEAD]].cmdPosition;
- *jointSpeed = pr2HeadIface->data->actuators[id-JointStart[HEAD]].cmdSpeed;
- pr2HeadIface->Unlock();
- }
- else
- {
- pr2Iface->Lock(1);
- *jointPosition = pr2Iface->data->actuators[id].cmdPosition;
- *jointSpeed = pr2Iface->data->actuators[id].cmdSpeed;
- pr2Iface->Unlock();
- }
+ *jointPosition = this->jointData[id].cmdPosition;
+ *jointSpeed = this->jointData[id].cmdSpeed;
return PR2_ALL_OK;
};
PR2_ERROR_CODE PR2HW::GetJointServoActual(PR2_JOINT_ID id, double *jointPosition, double *jointSpeed)
{
- if(IsHead(id))
- {
- pr2HeadIface->Lock(1);
- *jointPosition = pr2HeadIface->data->actuators[id-JointStart[HEAD]].actualPosition;
- *jointSpeed = pr2HeadIface->data->actuators[id-JointStart[HEAD]].actualSpeed;
- pr2HeadIface->Unlock();
- }
- else
- {
- pr2Iface->Lock(1);
- *jointPosition = pr2Iface->data->actuators[id].actualPosition;
- *jointSpeed = pr2Iface->data->actuators[id].actualSpeed;
- pr2Iface->Unlock();
- }
+ *jointPosition = this->jointData[id].actualPosition;
+ *jointSpeed = this->jointData[id].actualSpeed;
return PR2_ALL_OK;
};
PR2_ERROR_CODE PR2HW::GetJointPositionActual(PR2_JOINT_ID id, double *jointPosition, double *jointSpeed)
{
- if(IsHead(id))
- {
- pr2HeadIface->Lock(1);
- *jointPosition = pr2HeadIface->data->actuators[id-JointStart[HEAD]].actualPosition;
- *jointSpeed = pr2HeadIface->data->actuators[id-JointStart[HEAD]].actualSpeed;
- pr2HeadIface->Unlock();
- }
- else{
- pr2Iface->Lock(1);
- *jointPosition = pr2Iface->data->actuators[id].actualPosition;
- *jointSpeed = pr2Iface->data->actuators[id].actualSpeed;
- pr2Iface->Unlock();
- }
- return PR2_ALL_OK;
+ *jointPosition = this->jointData[id].actualPosition;
+ *jointSpeed = this->jointData[id].actualSpeed;
+ return PR2_ALL_OK;
};
PR2_ERROR_CODE PR2HW::SetJointTorque(PR2_JOINT_ID id, double torque)
{
- if(IsHead(id))
- {
- pr2HeadIface->Lock(1);
- pr2HeadIface->data->actuators[id-JointStart[HEAD]].cmdEffectorForce = torque;
- pr2HeadIface->Unlock();
- }
- else
- {
- pr2Iface->Lock(1);
- pr2Iface->data->actuators[id].cmdEffectorForce = torque;
- pr2Iface->Unlock();
- }
- return PR2_ALL_OK;
+ this->jointData[id].cmdEffectorForce = torque;
+ return PR2_ALL_OK;
};
PR2_ERROR_CODE PR2HW::GetJointTorqueCmd(PR2_JOINT_ID id, double *torque)
{
- if(IsHead(id))
- {
- pr2HeadIface->Lock(1);
- *torque = pr2HeadIface->data->actuators[id-JointStart[HEAD]].cmdEffectorForce;
- pr2HeadIface->Unlock();
- }
- else
- {
- pr2Iface->Lock(1);
- *torque = pr2Iface->data->actuators[id].cmdEffectorForce;
- pr2Iface->Unlock();
- }
- return PR2_ALL_OK;
+ *torque = this->jointData[id].cmdEffectorForce;
+ return PR2_ALL_OK;
};
PR2_ERROR_CODE PR2HW::GetJointTorqueActual(PR2_JOINT_ID id, double *torque)
{
- if(IsHead(id))
- {
- pr2HeadIface->Lock(1);
- *torque = pr2HeadIface->data->actuators[id-JointStart[HEAD]].actualEffectorForce;
- pr2HeadIface->Unlock();
- }
- else
- {
- pr2Iface->Lock(1);
- *torque = pr2Iface->data->actuators[id].actualEffectorForce;
- pr2Iface->Unlock();
- }
- return PR2_ALL_OK;
+ *torque = this->jointData[id].actualEffectorForce;
+ return PR2_ALL_OK;
};
PR2_ERROR_CODE PR2HW::SetJointSpeed(PR2_JOINT_ID id, double speed)
{
- if(IsHead(id))
- {
- pr2HeadIface->Lock(1);
- pr2HeadIface->data->actuators[id-JointStart[HEAD]].cmdSpeed = speed;
- pr2HeadIface->Unlock();
- }
- else
- {
- pr2Iface->Lock(1);
- pr2Iface->data->actuators[id].cmdSpeed = speed;
- pr2Iface->Unlock();
- }
- return PR2_ALL_OK;
+ this->jointData[id].cmdSpeed = speed;
+ return PR2_ALL_OK;
};
PR2_ERROR_CODE PR2HW::GetJointSpeedCmd(PR2_JOINT_ID id, double *speed)
{
- if(IsHead(id))
- {
- pr2HeadIface->Lock(1);
- *speed = pr2HeadIface->data->actuators[id-JointStart[HEAD]].cmdSpeed;
- pr2HeadIface->Unlock();
- }
- else{
- pr2Iface->Lock(1);
- *speed = pr2Iface->data->actuators[id].cmdSpeed;
- pr2Iface->Unlock();
- }
- return PR2_ALL_OK;
+ *speed = this->jointData[id].cmdSpeed;
+ return PR2_ALL_OK;
};
PR2_ERROR_CODE PR2HW::GetJointSpeedActual(PR2_JOINT_ID id, double *speed)
{
- if(IsHead(id))
- {
- pr2HeadIface->Lock(1);
- *speed = pr2HeadIface->data->actuators[id-JointStart[HEAD]].actualSpeed;
- pr2HeadIface->Unlock();
- }
- else{
- pr2Iface->Lock(1);
- *speed = pr2Iface->data->actuators[id].actualSpeed;
- pr2Iface->Unlock();
- }
- return PR2_ALL_OK;
+ *speed = this->jointData[id].cmdSpeed;
+ return PR2_ALL_OK;
};
PR2_ERROR_CODE PR2HW::GetLaserRanges(PR2_SENSOR_ID id,
@@ -803,34 +541,19 @@
}
};
-PR2_ERROR_CODE PR2HW::ClientWait()
-{
- // block until simulator update.
- client->Wait();
- return PR2_ALL_OK;
-}
-
-PR2_ERROR_CODE PR2HW::UpdateHW()
-{
-
- return PR2_ALL_OK;
-}
-
PR2_ERROR_CODE PR2HW::OpenGripper(PR2_MODEL_ID id,double gap,double force)
{
switch(id)
{
case PR2_LEFT_GRIPPER:
- pr2GripperLeftIface->data->cmd = GAZEBO_PR2GRIPPER_CMD_OPEN;
- pr2GripperLeftIface->data->cmdPositionRate = 1.0;
- pr2GripperLeftIface->data->cmdForce = force;
- pr2GripperLeftIface->data->cmdGap = gap;
- break;
case PR2_RIGHT_GRIPPER:
- pr2GripperRightIface->data->cmd = GAZEBO_PR2GRIPPER_CMD_OPEN;
- pr2GripperRightIface->data->cmdPositionRate = 1.0;
- pr2GripperRightIface->data->cmdForce = force;
- pr2GripperRightIface->data->cmdGap = gap;
+ for (int ii = JointStart[id];ii < JointEnd[ii];ii++)
+ {
+ this->jointData[ii].cmdMode = GAZEBO_PR2GRIPPER_CMD_OPEN;
+ this->jointData[ii].cmdSpeed = 1.0;
+ this->jointData[ii].cmdEffectorForce = force;
+ this->jointData[ii].cmdGap = gap;
+ }
break;
default:
break;
@@ -843,16 +566,11 @@
switch(id)
{
case PR2_LEFT_GRIPPER:
- pr2GripperLeftIface->data->cmd = GAZEBO_PR2GRIPPER_CMD_CLOSE;
- pr2GripperLeftIface->data->cmdPositionRate = 1.0;
- pr2GripperLeftIface->data->cmdForce = force;
- pr2GripperLeftIface->data->cmdGap = gap;
- break;
case PR2_RIGHT_GRIPPER:
- pr2GripperRightIface->data->cmd = GAZEBO_PR2GRIPPER_CMD_CLOSE;
- pr2GripperRightIface->data->cmdPositionRate = 1.0;
- pr2GripperRightIface->data->cmdForce = force;
- pr2GripperRightIface->data->cmdGap = gap;
+ this->jointData[id].cmdMode = GAZEBO_PR2GRIPPER_CMD_OPEN;
+ this->jointData[id].cmdSpeed = 1.0;
+ this->jointData[id].cmdEffectorForce = force;
+ this->jointData[id].cmdGap = gap;
break;
default:
break;
@@ -865,12 +583,12 @@
switch(id)
{
case PR2_LEFT_GRIPPER:
- *force = pr2GripperLeftIface->data->cmdForce;
- *gap = pr2GripperLeftIface->data->cmdGap ;
- break;
case PR2_RIGHT_GRIPPER:
- *force = pr2GripperRightIface->data->cmdForce;
- *gap = pr2GripperRightIface->data->cmdGap ;
+ for (int ii = JointStart[id];ii < JointEnd[ii];ii++)
+ {
+ *force = this->jointData[id].cmdEffectorForce;
+ *gap = this->jointData[id].cmdGap ;
+ }
break;
default:
break;
@@ -883,14 +601,9 @@
switch(id)
{
case PR2_LEFT_GRIPPER:
- *force = pr2GripperLeftIface->data->gripperForce; // FIXME: this is saturation force
- *gap = ( pr2GripperLeftIface->data->actualFingerPosition[0] - 0.015) // FIXME: not debugged
- +(-pr2GripperLeftIface->data->actualFingerPosition[1] + 0.015);
- break;
case PR2_RIGHT_GRIPPER:
- *force = pr2GripperRightIface->data->gripperForce; // FIXME: this is saturation force
- *gap = ( pr2GripperRightIface->data->actualFingerPosition[0] - 0.015) // FIXME: not debugged
- +(-pr2GripperRightIface->data->actualFingerPosition[1] + 0.015);
+ *force = this->jointData[id].cmdEffectorForce;
+ *gap = this->jointData[id].cmdGap ;
break;
default:
break;
@@ -903,14 +616,13 @@
switch(id)
{
case PR2_LEFT_GRIPPER:
- pr2GripperLeftIface->data->pGain = p;
- pr2GripperLeftIface->data->iGain = i;
- pr2GripperLeftIface->data->dGain = d;
- break;
case PR2_RIGHT_GRIPPER:
- pr2GripperRightIface->data->pGain = p;
- pr2GripperRightIface->data->iGain = i;
- pr2GripperRightIface->data->dGain = d;
+ for (int ii = JointStart[id];ii < JointEnd[ii];ii++)
+ {
+ this->jointData[id].pGain = p;
+ this->jointData[id].iGain = i;
+ this->jointData[id].dGain = d;
+ }
break;
default:
break;
@@ -1070,5 +782,133 @@
return PR2_ALL_OK;
};
+PR2_ERROR_CODE PR2HW::ClientWait()
+{
+ // block until simulator update.
+ client->Wait();
+ return PR2_ALL_OK;
+}
+PR2_ERROR_CODE PR2HW::UpdateHW()
+{
+ std::cout << "updating HW receive\n" << std::endl;
+ // receive data from hardware
+ for (int id = PR2::CASTER_FL_STEER; id < PR2::HEAD_PTZ_R_TILT; id++)
+ {
+ if(IsHead((PR2::PR2_JOINT_ID)id))
+ {
+ pr2HeadIface->Lock(1);
+ this->jointData[id].actualPosition = pr2HeadIface->data->actuators[id-JointStart[HEAD]].actualPosition ;
+ this->jointData[id].actualSpeed = pr2HeadIface->data->actuators[id-JointStart[HEAD]].actualSpeed ;
+ this->jointData[id].actualEffectorForce = pr2HeadIface->data->actuators[id-JointStart[HEAD]].actualEffectorForce;
+ pr2HeadIface->Unlock();
+ }
+ else if(IsGripperLeft((PR2::PR2_JOINT_ID)id))
+ {
+ pr2GripperLeftIface->Lock(1);
+ this->jointData[id].actualPosition = ( pr2GripperLeftIface->data->actualFingerPosition[0] - 0.015) // FIXME: not debugged
+ +(-pr2GripperLeftIface->data->actualFingerPosition[1] + 0.015);
+ this->jointData[id].actualGap = ( pr2GripperLeftIface->data->actualFingerPosition[0] - 0.015) // FIXME: not debugged
+ +(-pr2GripperLeftIface->data->actualFingerPosition[1] + 0.015);
+ this->jointData[id].actualSpeed = ( pr2GripperLeftIface->data->actualFingerPositionRate[0]) // FIXME: not debugged
+ +(-pr2GripperLeftIface->data->actualFingerPositionRate[1]);
+ this->jointData[id].actualSpeed = pr2GripperLeftIface->data->actualFingerPositionRate[0]; // FIXME: temporary numbers
+ this->jointData[id].actualEffectorForce = pr2GripperLeftIface->data->gripperForce;
+ this->jointData[id].actualFingerPosition[0] = pr2GripperLeftIface->data->actualFingerPosition[0];
+ this->jointData[id].actualFingerPosition[1] = pr2GripperLeftIface->data->actualFingerPosition[1];
+ this->jointData[id].actualFingerPositionRate[0] = pr2GripperLeftIface->data->actualFingerPositionRate[0];
+ this->jointData[id].actualFingerPositionRate[1] = pr2GripperLeftIface->data->actualFingerPositionRate[1];
+ pr2GripperLeftIface->Unlock();
+ }
+ else if(IsGripperRight((PR2::PR2_JOINT_ID...
[truncated message content] |
|
From: <ge...@us...> - 2008-07-11 06:38:09
|
Revision: 1474
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=1474&view=rev
Author: gerkey
Date: 2008-07-10 23:38:17 -0700 (Thu, 10 Jul 2008)
Log Message:
-----------
added rpath linker flags to packages with libraries, to support non-cmake builds of dependent executables
Modified Paths:
--------------
pkg/trunk/simulators/rosgazebo/manifest.xml
pkg/trunk/util/transforms/libTF/manifest.xml
pkg/trunk/util/transforms/rosTF/manifest.xml
pkg/trunk/util/wg_collision_space/manifest.xml
pkg/trunk/util/wg_robot_desc/manifest.xml
pkg/trunk/util/wg_robot_models/manifest.xml
pkg/trunk/visualization/wx_ros/wx_topic_display/manifest.xml
Modified: pkg/trunk/simulators/rosgazebo/manifest.xml
===================================================================
--- pkg/trunk/simulators/rosgazebo/manifest.xml 2008-07-11 06:24:41 UTC (rev 1473)
+++ pkg/trunk/simulators/rosgazebo/manifest.xml 2008-07-11 06:38:17 UTC (rev 1474)
@@ -11,6 +11,6 @@
<depend package="rosthread" />
<depend package="rosTF" />
<export>
- <cpp cflags="-I${prefix}/include" lflags="-L${prefix}/lib -lRosGazeboNode"/>
+ <cpp cflags="-I${prefix}/include" lflags="-Wl,-rpath,${prefix}/lib -L${prefix}/lib -lRosGazeboNode"/>
</export>
</package>
Modified: pkg/trunk/util/transforms/libTF/manifest.xml
===================================================================
--- pkg/trunk/util/transforms/libTF/manifest.xml 2008-07-11 06:24:41 UTC (rev 1473)
+++ pkg/trunk/util/transforms/libTF/manifest.xml 2008-07-11 06:38:17 UTC (rev 1474)
@@ -9,6 +9,6 @@
<url>http://pr.willowgarage.com</url>
<depend package="newmat10"/>
<export>
-<cpp cflags="-I${prefix}/include" lflags="-L${prefix}/lib -lTF -lnewmat"/>
+<cpp cflags="-I${prefix}/include" lflags="-Wl,-rpath,${prefix}/lib -L${prefix}/lib -lTF -lnewmat"/>
</export>
</package>
Modified: pkg/trunk/util/transforms/rosTF/manifest.xml
===================================================================
--- pkg/trunk/util/transforms/rosTF/manifest.xml 2008-07-11 06:24:41 UTC (rev 1473)
+++ pkg/trunk/util/transforms/rosTF/manifest.xml 2008-07-11 06:38:17 UTC (rev 1474)
@@ -8,7 +8,7 @@
<license>BSD</license>
<url>http://pr.willowgarage.com</url>
<export>
-<cpp cflags="-I${prefix}/include" lflags="-L${prefix}/lib -lrosTF"/>
+<cpp cflags="-I${prefix}/include" lflags="-Wl,-rpath,${prefix}/lib -L${prefix}/lib -lrosTF"/>
</export>
<depend package="libTF"/>
<depend package="roscpp"/>
Modified: pkg/trunk/util/wg_collision_space/manifest.xml
===================================================================
--- pkg/trunk/util/wg_collision_space/manifest.xml 2008-07-11 06:24:41 UTC (rev 1473)
+++ pkg/trunk/util/wg_collision_space/manifest.xml 2008-07-11 06:38:17 UTC (rev 1474)
@@ -11,7 +11,7 @@
<depend package="drawstuff"/>
<export>
- <cpp cflags="-I${prefix}/include" lflags="-L${prefix}/lib -lCollisionSpace"/>
+ <cpp cflags="-I${prefix}/include" lflags="-Wl,-rpath,${prefix}/lib -L${prefix}/lib -lCollisionSpace"/>
</export>
</package>
Modified: pkg/trunk/util/wg_robot_desc/manifest.xml
===================================================================
--- pkg/trunk/util/wg_robot_desc/manifest.xml 2008-07-11 06:24:41 UTC (rev 1473)
+++ pkg/trunk/util/wg_robot_desc/manifest.xml 2008-07-11 06:38:17 UTC (rev 1474)
@@ -12,7 +12,7 @@
<depend package="tinyxml"/>
<export>
- <cpp cflags="-I${prefix}/include" lflags="-L${prefix}/lib -lURDF"/>
+ <cpp cflags="-I${prefix}/include" lflags="-Wl,-rpath,${prefix}/lib -L${prefix}/lib -lURDF"/>
</export>
</package>
Modified: pkg/trunk/util/wg_robot_models/manifest.xml
===================================================================
--- pkg/trunk/util/wg_robot_models/manifest.xml 2008-07-11 06:24:41 UTC (rev 1473)
+++ pkg/trunk/util/wg_robot_models/manifest.xml 2008-07-11 06:38:17 UTC (rev 1474)
@@ -11,7 +11,7 @@
<depend package="libTF"/>
<export>
- <cpp cflags="-I${prefix}/include" lflags="-L${prefix}/lib -lRobotModels"/>
+ <cpp cflags="-I${prefix}/include" lflags="-Wl,-rpath,${prefix}/lib -L${prefix}/lib -lRobotModels"/>
</export>
</package>
Modified: pkg/trunk/visualization/wx_ros/wx_topic_display/manifest.xml
===================================================================
--- pkg/trunk/visualization/wx_ros/wx_topic_display/manifest.xml 2008-07-11 06:24:41 UTC (rev 1473)
+++ pkg/trunk/visualization/wx_ros/wx_topic_display/manifest.xml 2008-07-11 06:38:17 UTC (rev 1474)
@@ -8,7 +8,7 @@
<depend package="roscpp"/>
<depend package="std_msgs"/>
<export>
- <cpp cflags="-I${prefix}/src/" lflags="-L${prefix}/lib -lwxtopicdisplay"/>
+ <cpp cflags="-I${prefix}/src/" lflags="-Wl,-rpath,${prefix}/lib -L${prefix}/lib -lwxtopicdisplay"/>
</export>
</package>
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <jl...@us...> - 2008-07-11 09:19:47
|
Revision: 1483
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=1483&view=rev
Author: jleibs
Date: 2008-07-11 02:19:53 -0700 (Fri, 11 Jul 2008)
Log Message:
-----------
Splitting gui from the lower level logging headers so that megamaid doesn't depend on wx.
Modified Paths:
--------------
pkg/trunk/util/logging/manifest.xml
pkg/trunk/visualization/wx_ros/wx_topic_display/src/test/GenTestTopicDisplay.cpp
pkg/trunk/visualization/wx_ros/wx_topic_display/src/test/GenTestTopicDisplay.h
pkg/trunk/visualization/wx_ros/wx_topic_display/src/test/test_topic_display.fbp
pkg/trunk/visualization/wx_ros/wx_topic_display/src/wx_topic_display/GenTopicDisplay.cpp
pkg/trunk/visualization/wx_ros/wx_topic_display/src/wx_topic_display/GenTopicDisplay.h
Added Paths:
-----------
pkg/trunk/visualization/log_gui/
pkg/trunk/visualization/log_gui/CMakeLists.txt
pkg/trunk/visualization/log_gui/Makefile
pkg/trunk/visualization/log_gui/README
pkg/trunk/visualization/log_gui/bin/
pkg/trunk/visualization/log_gui/manifest.xml
pkg/trunk/visualization/log_gui/src/
Removed Paths:
-------------
pkg/trunk/util/logging/CMakeLists.txt
pkg/trunk/util/logging/Makefile
pkg/trunk/util/logging/README
pkg/trunk/util/logging/bin/
pkg/trunk/util/logging/src/
pkg/trunk/visualization/log_gui/Makefile
pkg/trunk/visualization/log_gui/README
pkg/trunk/visualization/log_gui/bin/
pkg/trunk/visualization/log_gui/include/
pkg/trunk/visualization/log_gui/manifest.xml
pkg/trunk/visualization/log_gui/src/
Deleted: pkg/trunk/util/logging/CMakeLists.txt
===================================================================
--- pkg/trunk/util/logging/CMakeLists.txt 2008-07-11 09:13:21 UTC (rev 1482)
+++ pkg/trunk/util/logging/CMakeLists.txt 2008-07-11 09:19:53 UTC (rev 1483)
@@ -1,27 +0,0 @@
-cmake_minimum_required(VERSION 2.6)
-include(rosbuild)
-rospack(logging)
-
-set(wxWidgets_USE_LIBS base core)
-
-find_package(wxWidgets REQUIRED)
-
-include(${wxWidgets_USE_FILE})
-include_directories(${wxWidgets_INCLUDE_DIRS})
-
-find_program(WXFORMBUILDER NAMES wxformbuilder)
-
-if(WXFORMBUILDER)
- add_custom_command(OUTPUT ${PROJECT_SOURCE_DIR}/src/log_gui/GenLogGui.cpp
- ${PROJECT_SOURCE_DIR}/src/log_gui/GenLogGui.h
- COMMAND ${WXFORMBUILDER} -g ${PROJECT_SOURCE_DIR}/src/log_gui/log_gui.fbp
- DEPENDS src/log_gui/log_gui.fbp)
-endif(WXFORMBUILDER)
-
-set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
-
-rospack_add_executable(log_gui src/log_gui/LogGui.cpp
- src/log_gui/GenLogGui.cpp)
-
-target_link_libraries(log_gui wxtopicdisplay ${wxWidgets_LIBRARIES})
-
Deleted: pkg/trunk/util/logging/Makefile
===================================================================
--- pkg/trunk/util/logging/Makefile 2008-07-11 09:13:21 UTC (rev 1482)
+++ pkg/trunk/util/logging/Makefile 2008-07-11 09:19:53 UTC (rev 1483)
@@ -1 +0,0 @@
-include $(shell rospack find mk)/cmake.mk
Deleted: pkg/trunk/util/logging/README
===================================================================
--- pkg/trunk/util/logging/README 2008-07-11 09:13:21 UTC (rev 1482)
+++ pkg/trunk/util/logging/README 2008-07-11 09:19:53 UTC (rev 1483)
@@ -1,6 +0,0 @@
-You need to have wxWidgets installed on your box for this to compile.
-
-To do this on Ubuntu 08.04, type this:
-
-sudo apt-get install wxgtk2.8-dev
-
Modified: pkg/trunk/util/logging/manifest.xml
===================================================================
--- pkg/trunk/util/logging/manifest.xml 2008-07-11 09:13:21 UTC (rev 1482)
+++ pkg/trunk/util/logging/manifest.xml 2008-07-11 09:19:53 UTC (rev 1483)
@@ -1,15 +1,11 @@
<package>
-<description brief="System logger">
-Gui for selecting and logging topics from a running system.
-Currently does all logging in a single centralized filesystem, so don't
-expect to use it to log video over a wireless link.
+<description brief="Logging headers">
</description>
-<author>Eric Berger be...@wi..., Morgan Quigley mqu...@wi...</author>
+<author>Jeremy Leibs be...@wi..., Morgan Quigley mqu...@wi...</author>
<license>BSD</license>
<url>http://pr.willowgarage.com</url>
<depend package="roscpp"/>
<depend package="std_msgs"/>
-<depend package="wx_topic_display"/>
<export>
<cpp cflags="-I${prefix}/include"/>
</export>
Copied: pkg/trunk/visualization/log_gui (from rev 1467, pkg/trunk/util/logging)
Copied: pkg/trunk/visualization/log_gui/CMakeLists.txt (from rev 1482, pkg/trunk/util/logging/CMakeLists.txt)
===================================================================
--- pkg/trunk/visualization/log_gui/CMakeLists.txt (rev 0)
+++ pkg/trunk/visualization/log_gui/CMakeLists.txt 2008-07-11 09:19:53 UTC (rev 1483)
@@ -0,0 +1,27 @@
+cmake_minimum_required(VERSION 2.6)
+include(rosbuild)
+rospack(logging)
+
+set(wxWidgets_USE_LIBS base core)
+
+find_package(wxWidgets REQUIRED)
+
+include(${wxWidgets_USE_FILE})
+include_directories(${wxWidgets_INCLUDE_DIRS})
+
+find_program(WXFORMBUILDER NAMES wxformbuilder)
+
+if(WXFORMBUILDER)
+ add_custom_command(OUTPUT ${PROJECT_SOURCE_DIR}/src/log_gui/GenLogGui.cpp
+ ${PROJECT_SOURCE_DIR}/src/log_gui/GenLogGui.h
+ COMMAND ${WXFORMBUILDER} -g ${PROJECT_SOURCE_DIR}/src/log_gui/log_gui.fbp
+ DEPENDS src/log_gui/log_gui.fbp)
+endif(WXFORMBUILDER)
+
+set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
+
+rospack_add_executable(log_gui src/log_gui/LogGui.cpp
+ src/log_gui/GenLogGui.cpp)
+
+target_link_libraries(log_gui wxtopicdisplay ${wxWidgets_LIBRARIES})
+
Deleted: pkg/trunk/visualization/log_gui/Makefile
===================================================================
--- pkg/trunk/util/logging/Makefile 2008-07-11 03:07:02 UTC (rev 1467)
+++ pkg/trunk/visualization/log_gui/Makefile 2008-07-11 09:19:53 UTC (rev 1483)
@@ -1,3 +0,0 @@
-SUBDIRS = src
-include $(shell rospack find mk)/recurse_subdirs.mk
-
Copied: pkg/trunk/visualization/log_gui/Makefile (from rev 1482, pkg/trunk/util/logging/Makefile)
===================================================================
--- pkg/trunk/visualization/log_gui/Makefile (rev 0)
+++ pkg/trunk/visualization/log_gui/Makefile 2008-07-11 09:19:53 UTC (rev 1483)
@@ -0,0 +1 @@
+include $(shell rospack find mk)/cmake.mk
Deleted: pkg/trunk/visualization/log_gui/README
===================================================================
--- pkg/trunk/util/logging/README 2008-07-11 03:07:02 UTC (rev 1467)
+++ pkg/trunk/visualization/log_gui/README 2008-07-11 09:19:53 UTC (rev 1483)
@@ -1,6 +0,0 @@
-You need to have wxWidgets installed on your box for this to compile.
-
-To do this on Ubuntu 08.04, type this:
-
-sudo apt-get install wxgtk2.8-dev
-
Copied: pkg/trunk/visualization/log_gui/README (from rev 1482, pkg/trunk/util/logging/README)
===================================================================
--- pkg/trunk/visualization/log_gui/README (rev 0)
+++ pkg/trunk/visualization/log_gui/README 2008-07-11 09:19:53 UTC (rev 1483)
@@ -0,0 +1,6 @@
+You need to have wxWidgets installed on your box for this to compile.
+
+To do this on Ubuntu 08.04, type this:
+
+sudo apt-get install wxgtk2.8-dev
+
Copied: pkg/trunk/visualization/log_gui/bin (from rev 1482, pkg/trunk/util/logging/bin)
Deleted: pkg/trunk/visualization/log_gui/manifest.xml
===================================================================
--- pkg/trunk/util/logging/manifest.xml 2008-07-11 03:07:02 UTC (rev 1467)
+++ pkg/trunk/visualization/log_gui/manifest.xml 2008-07-11 09:19:53 UTC (rev 1483)
@@ -1,16 +0,0 @@
-<package>
-<description brief="System logger">
-Gui for selecting and logging topics from a running system.
-Currently does all logging in a single centralized filesystem, so don't
-expect to use it to log video over a wireless link.
-</description>
-<author>Eric Berger be...@wi..., Morgan Quigley mqu...@wi...</author>
-<license>BSD</license>
-<url>http://pr.willowgarage.com</url>
-<depend package="roscpp"/>
-<depend package="std_msgs"/>
-<export>
- <cpp cflags="-I${prefix}/include"/>
-</export>
-</package>
-
Copied: pkg/trunk/visualization/log_gui/manifest.xml (from rev 1482, pkg/trunk/util/logging/manifest.xml)
===================================================================
--- pkg/trunk/visualization/log_gui/manifest.xml (rev 0)
+++ pkg/trunk/visualization/log_gui/manifest.xml 2008-07-11 09:19:53 UTC (rev 1483)
@@ -0,0 +1,15 @@
+<package>
+<description brief="System logger">
+Gui for selecting and logging topics from a running system.
+Currently does all logging in a single centralized filesystem, so don't
+expect to use it to log video over a wireless link.
+</description>
+<author>Eric Berger be...@wi..., Morgan Quigley mqu...@wi...</author>
+<license>BSD</license>
+<url>http://pr.willowgarage.com</url>
+<depend package="roscpp"/>
+<depend package="std_msgs"/>
+<depend package="logging"/>
+<depend package="wx_topic_display"/>
+</package>
+
Copied: pkg/trunk/visualization/log_gui/src (from rev 1482, pkg/trunk/util/logging/src)
Modified: pkg/trunk/visualization/wx_ros/wx_topic_display/src/test/GenTestTopicDisplay.cpp
===================================================================
--- pkg/trunk/visualization/wx_ros/wx_topic_display/src/test/GenTestTopicDisplay.cpp 2008-07-11 09:13:21 UTC (rev 1482)
+++ pkg/trunk/visualization/wx_ros/wx_topic_display/src/test/GenTestTopicDisplay.cpp 2008-07-11 09:19:53 UTC (rev 1483)
@@ -1,5 +1,5 @@
///////////////////////////////////////////////////////////////////////////
-// C++ code generated with wxFormBuilder (version Apr 16 2008)
+// C++ code generated with wxFormBuilder (version Apr 21 2008)
// http://www.wxformbuilder.org/
//
// PLEASE DO "NOT" EDIT THIS FILE!
@@ -14,7 +14,7 @@
this->SetSizeHints( wxDefaultSize, wxDefaultSize );
wxStaticBoxSizer* sbSizer1;
- sbSizer1 = new wxStaticBoxSizer( new wxStaticBox( this, wxID_ANY, wxT("label") ), wxHORIZONTAL );
+ sbSizer1 = new wxStaticBoxSizer( new wxStaticBox( this, wxID_ANY, wxEmptyString ), wxHORIZONTAL );
topicPanel = new wxPanel( this, wxID_ANY, wxDefaultPosition, wxDefaultSize, wxTAB_TRAVERSAL );
sbSizer1->Add( topicPanel, 1, wxALL|wxEXPAND, 5 );
Modified: pkg/trunk/visualization/wx_ros/wx_topic_display/src/test/GenTestTopicDisplay.h
===================================================================
--- pkg/trunk/visualization/wx_ros/wx_topic_display/src/test/GenTestTopicDisplay.h 2008-07-11 09:13:21 UTC (rev 1482)
+++ pkg/trunk/visualization/wx_ros/wx_topic_display/src/test/GenTestTopicDisplay.h 2008-07-11 09:19:53 UTC (rev 1483)
@@ -1,5 +1,5 @@
///////////////////////////////////////////////////////////////////////////
-// C++ code generated with wxFormBuilder (version Apr 16 2008)
+// C++ code generated with wxFormBuilder (version Apr 21 2008)
// http://www.wxformbuilder.org/
//
// PLEASE DO "NOT" EDIT THIS FILE!
Modified: pkg/trunk/visualization/wx_ros/wx_topic_display/src/test/test_topic_display.fbp
===================================================================
--- pkg/trunk/visualization/wx_ros/wx_topic_display/src/test/test_topic_display.fbp 2008-07-11 09:13:21 UTC (rev 1482)
+++ pkg/trunk/visualization/wx_ros/wx_topic_display/src/test/test_topic_display.fbp 2008-07-11 09:19:53 UTC (rev 1483)
@@ -72,7 +72,7 @@
<event name="OnUpdateUI"></event>
<object class="wxStaticBoxSizer" expanded="1">
<property name="id">wxID_ANY</property>
- <property name="label">label</property>
+ <property name="label"></property>
<property name="minimum_size"></property>
<property name="name">sbSizer1</property>
<property name="orient">wxHORIZONTAL</property>
Modified: pkg/trunk/visualization/wx_ros/wx_topic_display/src/wx_topic_display/GenTopicDisplay.cpp
===================================================================
--- pkg/trunk/visualization/wx_ros/wx_topic_display/src/wx_topic_display/GenTopicDisplay.cpp 2008-07-11 09:13:21 UTC (rev 1482)
+++ pkg/trunk/visualization/wx_ros/wx_topic_display/src/wx_topic_display/GenTopicDisplay.cpp 2008-07-11 09:19:53 UTC (rev 1483)
@@ -1,5 +1,5 @@
///////////////////////////////////////////////////////////////////////////
-// C++ code generated with wxFormBuilder (version Apr 16 2008)
+// C++ code generated with wxFormBuilder (version Apr 21 2008)
// http://www.wxformbuilder.org/
//
// PLEASE DO "NOT" EDIT THIS FILE!
Modified: pkg/trunk/visualization/wx_ros/wx_topic_display/src/wx_topic_display/GenTopicDisplay.h
===================================================================
--- pkg/trunk/visualization/wx_ros/wx_topic_display/src/wx_topic_display/GenTopicDisplay.h 2008-07-11 09:13:21 UTC (rev 1482)
+++ pkg/trunk/visualization/wx_ros/wx_topic_display/src/wx_topic_display/GenTopicDisplay.h 2008-07-11 09:19:53 UTC (rev 1483)
@@ -1,5 +1,5 @@
///////////////////////////////////////////////////////////////////////////
-// C++ code generated with wxFormBuilder (version Apr 16 2008)
+// C++ code generated with wxFormBuilder (version Apr 21 2008)
// http://www.wxformbuilder.org/
//
// PLEASE DO "NOT" EDIT THIS FILE!
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <ge...@us...> - 2008-07-12 01:32:11
|
Revision: 1515
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=1515&view=rev
Author: gerkey
Date: 2008-07-11 18:32:20 -0700 (Fri, 11 Jul 2008)
Log Message:
-----------
Updated CMake files to respect the fact that they don't inherit their own exported build flags.
Modified Paths:
--------------
pkg/trunk/drivers/robot/pr2/libpr2API/CMakeLists.txt
pkg/trunk/drivers/robot/pr2/libpr2HW/CMakeLists.txt
pkg/trunk/nav/wavefront_player/CMakeLists.txt
pkg/trunk/simulators/rosgazebo/CMakeLists.txt
pkg/trunk/util/transforms/libTF/CMakeLists.txt
pkg/trunk/util/transforms/rosTF/CMakeLists.txt
pkg/trunk/util/wg_collision_space/CMakeLists.txt
pkg/trunk/util/wg_robot_desc/CMakeLists.txt
pkg/trunk/util/wg_robot_models/CMakeLists.txt
Modified: pkg/trunk/drivers/robot/pr2/libpr2API/CMakeLists.txt
===================================================================
--- pkg/trunk/drivers/robot/pr2/libpr2API/CMakeLists.txt 2008-07-12 01:07:47 UTC (rev 1514)
+++ pkg/trunk/drivers/robot/pr2/libpr2API/CMakeLists.txt 2008-07-12 01:32:20 UTC (rev 1515)
@@ -5,10 +5,14 @@
#set(ROS_BUILD_STATIC_EXES true)
rospack_add_library(pr2API src/pr2API.cpp)
rospack_add_executable(test_libpr2API src/test/test_pr2API.cpp)
+target_link_libraries(test_libpr2API pr2API)
rospack_add_executable(torque_test src/test/torque_test.cpp)
+target_link_libraries(torque_test pr2API)
rospack_add_executable(pick_object src/test/pick_object.cpp)
+target_link_libraries(pick_object pr2API)
rospack_add_executable(test_kin_controllers src/test/test_kin_controllers.cpp src/kinematic_controllers.cpp)
+target_link_libraries(test_kin_controllers pr2API)
rospack_add_executable(pr2_kin_kdl src/test/pr2_kin_kdl.cpp)
+target_link_libraries(pr2_kin_kdl pr2API)
rospack_add_executable(test_FK src/test/test_FK.cpp)
-
-
+target_link_libraries(test_FK pr2API)
Modified: pkg/trunk/drivers/robot/pr2/libpr2HW/CMakeLists.txt
===================================================================
--- pkg/trunk/drivers/robot/pr2/libpr2HW/CMakeLists.txt 2008-07-12 01:07:47 UTC (rev 1514)
+++ pkg/trunk/drivers/robot/pr2/libpr2HW/CMakeLists.txt 2008-07-12 01:32:20 UTC (rev 1515)
@@ -5,3 +5,4 @@
#set(ROS_BUILD_STATIC_EXES true)
rospack_add_library(pr2HW src/pr2HW.cpp)
rospack_add_executable(test_pr2HW src/test/test_pr2HW.cpp)
+target_link_libraries(test_pr2HW pr2HW)
Modified: pkg/trunk/nav/wavefront_player/CMakeLists.txt
===================================================================
--- pkg/trunk/nav/wavefront_player/CMakeLists.txt 2008-07-12 01:07:47 UTC (rev 1514)
+++ pkg/trunk/nav/wavefront_player/CMakeLists.txt 2008-07-12 01:32:20 UTC (rev 1515)
@@ -9,3 +9,4 @@
rospack_add_executable(send_goal send_goal.cc)
rospack_add_executable(query_wavefront query_wavefront.cpp)
+include_directories(srv/cpp)
Modified: pkg/trunk/simulators/rosgazebo/CMakeLists.txt
===================================================================
--- pkg/trunk/simulators/rosgazebo/CMakeLists.txt 2008-07-12 01:07:47 UTC (rev 1514)
+++ pkg/trunk/simulators/rosgazebo/CMakeLists.txt 2008-07-12 01:32:20 UTC (rev 1515)
@@ -4,4 +4,6 @@
add_definitions(-Wall)
rospack_add_library(RosGazeboNode src/RosGazeboNode.cpp)
rospack_add_executable(rosgazebo src/rosgazebo.cc)
+target_link_libraries(rosgazebo RosGazeboNode)
rospack_add_executable(rosgazebowriter src/rosgazebowriter.cc)
+target_link_libraries(rosgazebowriter RosGazeboNode)
Modified: pkg/trunk/util/transforms/libTF/CMakeLists.txt
===================================================================
--- pkg/trunk/util/transforms/libTF/CMakeLists.txt 2008-07-12 01:07:47 UTC (rev 1514)
+++ pkg/trunk/util/transforms/libTF/CMakeLists.txt 2008-07-12 01:32:20 UTC (rev 1515)
@@ -4,6 +4,10 @@
add_definitions(-Wall)
rospack_add_library(TF src/libTF.cpp src/Pose3DCache.cpp src/Pose3D.cpp)
rospack_add_executable(test src/test/main.cpp)
+target_link_libraries(test TF)
rospack_add_executable(testtf src/test/testtf.cc)
+target_link_libraries(testtf TF)
rospack_add_executable(test_interp src/test/test_interp.cc)
+target_link_libraries(test_interp TF)
rospack_add_executable(testMatrix src/test/testMatrix.cc)
+target_link_libraries(testMatrix TF)
Modified: pkg/trunk/util/transforms/rosTF/CMakeLists.txt
===================================================================
--- pkg/trunk/util/transforms/rosTF/CMakeLists.txt 2008-07-12 01:07:47 UTC (rev 1514)
+++ pkg/trunk/util/transforms/rosTF/CMakeLists.txt 2008-07-12 01:32:20 UTC (rev 1515)
@@ -4,4 +4,6 @@
add_definitions(-Wall)
rospack_add_library(rosTF src/rosTF.cpp)
rospack_add_executable(testServer src/testServer.cc)
+target_link_libraries(testServer rosTF)
rospack_add_executable(testClient src/testClient.cc)
+target_link_libraries(testClient rosTF)
Modified: pkg/trunk/util/wg_collision_space/CMakeLists.txt
===================================================================
--- pkg/trunk/util/wg_collision_space/CMakeLists.txt 2008-07-12 01:07:47 UTC (rev 1514)
+++ pkg/trunk/util/wg_collision_space/CMakeLists.txt 2008-07-12 01:32:20 UTC (rev 1515)
@@ -1,10 +1,9 @@
cmake_minimum_required(VERSION 2.6)
include(rosbuild)
rospack(wg_collision_space)
-include_directories(${wg_collision_space_INCLUDE_DIRS})
rospack_add_library(CollisionSpace src/collisionspace/kinematicODE.cpp
src/collisionspace/kinematicSOLID.cpp
src/collisionspace/environmentODE.cpp
src/collisionspace/environmentSOLID.cpp)
-link_directories(${wg_collision_space_LIBRARY_DIRS})
rospack_add_executable(test_kinematic_ode src/test/test_kinematic_ode.cpp)
+target_link_libraries(test_kinematic_ode CollisionSpace)
Modified: pkg/trunk/util/wg_robot_desc/CMakeLists.txt
===================================================================
--- pkg/trunk/util/wg_robot_desc/CMakeLists.txt 2008-07-12 01:07:47 UTC (rev 1514)
+++ pkg/trunk/util/wg_robot_desc/CMakeLists.txt 2008-07-12 01:32:20 UTC (rev 1515)
@@ -1,7 +1,6 @@
cmake_minimum_required(VERSION 2.6)
include(rosbuild)
rospack(wg_robot_desc)
-include_directories(${wg_robot_desc_INCLUDE_DIRS})
rospack_add_library(URDF src/urdf/URDF.cpp src/urdf/MathExpression.cpp)
-link_directories(${wg_robot_desc_LIBRARY_DIRS})
rospack_add_executable(parse src/test/parse.cpp)
+target_link_libraries(parse URDF)
Modified: pkg/trunk/util/wg_robot_models/CMakeLists.txt
===================================================================
--- pkg/trunk/util/wg_robot_models/CMakeLists.txt 2008-07-12 01:07:47 UTC (rev 1514)
+++ pkg/trunk/util/wg_robot_models/CMakeLists.txt 2008-07-12 01:32:20 UTC (rev 1515)
@@ -1,5 +1,4 @@
cmake_minimum_required(VERSION 2.6)
include(rosbuild)
rospack(wg_robot_models)
-include_directories(${wg_robot_models_INCLUDE_DIRS})
rospack_add_library(RobotModels src/robotmodels/kinematic.cpp)
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <MP...@us...> - 2008-07-14 19:48:04
|
Revision: 1536
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=1536&view=rev
Author: MPPics
Date: 2008-07-14 12:48:05 -0700 (Mon, 14 Jul 2008)
Log Message:
-----------
added -Wl and -rpath flags.
Modified Paths:
--------------
pkg/trunk/drivers/robot/pr2/libpr2API/manifest.xml
pkg/trunk/drivers/robot/pr2/libpr2HW/manifest.xml
pkg/trunk/simulators/gazebo_plugin/manifest.xml
Modified: pkg/trunk/drivers/robot/pr2/libpr2API/manifest.xml
===================================================================
--- pkg/trunk/drivers/robot/pr2/libpr2API/manifest.xml 2008-07-14 18:20:20 UTC (rev 1535)
+++ pkg/trunk/drivers/robot/pr2/libpr2API/manifest.xml 2008-07-14 19:48:05 UTC (rev 1536)
@@ -13,8 +13,9 @@
<depend package="libKinematics"/>
<depend package="libKDL"/>
<depend package="rosTF"/>
+<depend package="freeimage"/>
<export>
-<cpp cflags="-I${prefix}/include" lflags="-L${prefix}/lib -lpr2API"/>
+<cpp cflags="-I${prefix}/include" lflags="-Wl,-rpath,${prefix}/lib -L${prefix}/lib -lpr2API"/>
</export>
</package>
Modified: pkg/trunk/drivers/robot/pr2/libpr2HW/manifest.xml
===================================================================
--- pkg/trunk/drivers/robot/pr2/libpr2HW/manifest.xml 2008-07-14 18:20:20 UTC (rev 1535)
+++ pkg/trunk/drivers/robot/pr2/libpr2HW/manifest.xml 2008-07-14 19:48:05 UTC (rev 1536)
@@ -1,5 +1,5 @@
<package>
-<description brief='API for baic PR2 Hardware controls'>
+<description brief='API for basic PR2 Hardware controls'>
An API for baic PR2 Hardware controls: enable, disable, set gains, PD controllers, talk to hardware via UpdateHW() call.
@@ -12,7 +12,7 @@
<depend package="pr2Core"/>
<depend package="libKinematics"/>
<export>
-<cpp cflags="-I${prefix}/include" lflags="-L${prefix}/lib -lpr2HW"/>
+<cpp cflags="-I${prefix}/include" lflags="-Wl,-rpath,${prefix}/lib -L${prefix}/lib -lpr2HW"/>
</export>
</package>
Modified: pkg/trunk/simulators/gazebo_plugin/manifest.xml
===================================================================
--- pkg/trunk/simulators/gazebo_plugin/manifest.xml 2008-07-14 18:20:20 UTC (rev 1535)
+++ pkg/trunk/simulators/gazebo_plugin/manifest.xml 2008-07-14 19:48:05 UTC (rev 1536)
@@ -14,6 +14,6 @@
<depend package="newmat10"/>
<depend package="genericControllers"/>
<export>
-<cpp cflags="-I${prefix}/include" lflags="-L${prefix}/lib -lPr2_Actarray -lPr2_Gripper -lP3D"/>
+<cpp cflags="-I${prefix}/include" lflags="-Wl,-rpath,${prefix}/lib -L${prefix}/lib -lPr2_Actarray -lPr2_Gripper -lP3D"/>
</export>
</package>
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <hsu...@us...> - 2008-07-14 23:03:19
|
Revision: 1567
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=1567&view=rev
Author: hsujohnhsu
Date: 2008-07-14 16:03:26 -0700 (Mon, 14 Jul 2008)
Log Message:
-----------
added empty directories structures per meeting minutes 2007-07-14
Added Paths:
-----------
pkg/trunk/mechanism/mechanism_control/
pkg/trunk/mechanism/robot_model/
pkg/trunk/mechanism/robot_model/joint/
pkg/trunk/mechanism/robot_model/link/
pkg/trunk/mechanism/robot_model/transmission/
pkg/trunk/robot/
pkg/trunk/robot/pr2_etherCAT/
pkg/trunk/robot/pr2_etherDrive/
pkg/trunk/robot/pr2_gazebo/
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <hsu...@us...> - 2008-07-14 23:41:38
|
Revision: 1579
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=1579&view=rev
Author: hsujohnhsu
Date: 2008-07-14 16:41:48 -0700 (Mon, 14 Jul 2008)
Log Message:
-----------
moved gazebo_plugin to drivers.
Added Paths:
-----------
pkg/trunk/drivers/simulator/
pkg/trunk/drivers/simulator/gazebo_plugin/
Removed Paths:
-------------
pkg/trunk/simulators/gazebo_plugin/
Copied: pkg/trunk/drivers/simulator/gazebo_plugin (from rev 1576, pkg/trunk/simulators/gazebo_plugin)
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <hsu...@us...> - 2008-07-15 19:26:09
|
Revision: 1610
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=1610&view=rev
Author: hsujohnhsu
Date: 2008-07-15 12:26:13 -0700 (Tue, 15 Jul 2008)
Log Message:
-----------
changed texture to match the wg wall model.
Modified Paths:
--------------
pkg/trunk/demos/2dnav-gazebo/world/Media/materials/scripts/pr2.material
pkg/trunk/demos/2dnav-gazebo/world/willow-walls.model
pkg/trunk/simulators/tools/gazebo_map_extruder/gazebo_map_extruder.cc
Modified: pkg/trunk/demos/2dnav-gazebo/world/Media/materials/scripts/pr2.material
===================================================================
--- pkg/trunk/demos/2dnav-gazebo/world/Media/materials/scripts/pr2.material 2008-07-15 18:44:46 UTC (rev 1609)
+++ pkg/trunk/demos/2dnav-gazebo/world/Media/materials/scripts/pr2.material 2008-07-15 19:26:13 UTC (rev 1610)
@@ -12,3 +12,18 @@
}
}
}
+
+material PR2/wall_texture
+{
+ technique
+ {
+ pass
+ {
+ texture_unit
+ {
+ texture willowMap.png
+ tex_address_mode clamp
+ }
+ }
+ }
+}
Modified: pkg/trunk/demos/2dnav-gazebo/world/willow-walls.model
===================================================================
--- pkg/trunk/demos/2dnav-gazebo/world/willow-walls.model 2008-07-15 18:44:46 UTC (rev 1609)
+++ pkg/trunk/demos/2dnav-gazebo/world/willow-walls.model 2008-07-15 19:26:13 UTC (rev 1610)
@@ -11190,7 +11190,7 @@
<kd>1.0</kd>
<normal>0 0 1</normal>
<size>51.300 51.300</size>
-<material>PR2/floor_texture</material>
+<material>PR2/wall_texture</material>
</geom:plane>
</body:plane>
</model:physical>
Modified: pkg/trunk/simulators/tools/gazebo_map_extruder/gazebo_map_extruder.cc
===================================================================
--- pkg/trunk/simulators/tools/gazebo_map_extruder/gazebo_map_extruder.cc 2008-07-15 18:44:46 UTC (rev 1609)
+++ pkg/trunk/simulators/tools/gazebo_map_extruder/gazebo_map_extruder.cc 2008-07-15 19:26:13 UTC (rev 1610)
@@ -138,7 +138,7 @@
"<kd>1.0</kd>\n"
"<normal>0 0 1</normal>\n"
"<size>%.3f %.3f</size>\n"
- "<material>PR2/floor_texture</material>\n"
+ "<material>PR2/wall_texture</material>\n"
"</geom:plane>\n"
"</body:plane>\n"
"</model:physical>\n",
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
Thanks for helping keep SourceForge clean.
X