Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots: [217] pkg/branches/rosbus
|
From: <jl...@us...> - 2008-04-28 23:25:08
|
Revision: 217
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=217&view=rev
Author: jleibs
Date: 2008-04-28 16:25:10 -0700 (Mon, 28 Apr 2008)
Log Message:
-----------
Merging in changes that have occured since branch... time to upgrade to rosbus.
Modified Paths:
--------------
pkg/branches/rosbus/etherdrive/build.yaml
pkg/branches/rosbus/etherdrive/include/etherdrive/etherdrive.h
pkg/branches/rosbus/etherdrive/src/etherdrive_control/etherdrive_control.cpp
pkg/branches/rosbus/etherdrive/src/libetherdrive/etherdrive.cpp
pkg/branches/rosbus/etherdrive/test/test_etherdrive/test_etherdrive.cpp
pkg/branches/rosbus/libTF/include/libTF/Quaternion3D.h
pkg/branches/rosbus/libTF/include/libTF/libTF.h
pkg/branches/rosbus/libTF/src/simple/Quaternion3D.cpp
pkg/branches/rosbus/libTF/src/simple/libTF.cpp
pkg/branches/rosbus/libTF/src/test/main.cpp
pkg/branches/rosbus/tilting_laser/manifest.xml
pkg/branches/rosbus/tilting_laser/nodes/launch_tl
pkg/branches/rosbus/tilting_laser/src/tilting_laser/tilting_laser.cpp
Added Paths:
-----------
pkg/branches/rosbus/controllers/
pkg/branches/rosbus/etherdrive/test/test_etherdrive2/
pkg/branches/rosbus/etherdrive/test/test_etherdrive2/Makefile
pkg/branches/rosbus/etherdrive/test/test_etherdrive2/test_etherdrive2.cpp
pkg/branches/rosbus/kdtree/
pkg/branches/rosbus/kdtree/Makefile
pkg/branches/rosbus/kdtree/include/
pkg/branches/rosbus/kdtree/include/kdtree/
pkg/branches/rosbus/kdtree/kdtree2.tar.gz
pkg/branches/rosbus/kdtree/lib/
pkg/branches/rosbus/kdtree/manifest.xml
pkg/branches/rosbus/kdtree/rosbuild
pkg/branches/rosbus/kdtree/test/
pkg/branches/rosbus/kdtree/test/Makefile
pkg/branches/rosbus/kdtree/test/test.cpp
pkg/branches/rosbus/libTF/doc/
pkg/branches/rosbus/libTF/doc/Makefile
pkg/branches/rosbus/libTF/doc/README.txt
pkg/branches/rosbus/libTF/doc/libTF_Manual.bib
pkg/branches/rosbus/libTF/doc/libTF_Manual.tex
Removed Paths:
-------------
pkg/branches/rosbus/etherdrive/include/etherdrive.h
pkg/branches/rosbus/etherdrive/test/test_etherdrive2/Makefile
pkg/branches/rosbus/etherdrive/test/test_etherdrive2/test_etherdrive2.cpp
pkg/branches/rosbus/kdtree/Makefile
pkg/branches/rosbus/kdtree/include/
pkg/branches/rosbus/kdtree/include/kdtree/
pkg/branches/rosbus/kdtree/kdtree2.tar.gz
pkg/branches/rosbus/kdtree/lib/
pkg/branches/rosbus/kdtree/manifest.xml
pkg/branches/rosbus/kdtree/rosbuild
pkg/branches/rosbus/kdtree/test/
pkg/branches/rosbus/kdtree/test/Makefile
pkg/branches/rosbus/kdtree/test/test.cpp
pkg/branches/rosbus/libTF/doc/Makefile
pkg/branches/rosbus/libTF/doc/README.txt
pkg/branches/rosbus/libTF/doc/libTF_Manual.bib
pkg/branches/rosbus/libTF/doc/libTF_Manual.tex
Copied: pkg/branches/rosbus/controllers (from rev 216, pkg/trunk/controllers)
Modified: pkg/branches/rosbus/etherdrive/build.yaml
===================================================================
--- pkg/branches/rosbus/etherdrive/build.yaml 2008-04-28 23:13:16 UTC (rev 216)
+++ pkg/branches/rosbus/etherdrive/build.yaml 2008-04-28 23:25:10 UTC (rev 217)
@@ -3,4 +3,5 @@
- src/libetherdrive
- src/etherdrive_control
- test/test_etherdrive
+ - test/test_etherdrive2
Modified: pkg/branches/rosbus/etherdrive/include/etherdrive/etherdrive.h
===================================================================
--- pkg/branches/rosbus/etherdrive/include/etherdrive/etherdrive.h 2008-04-28 23:13:16 UTC (rev 216)
+++ pkg/branches/rosbus/etherdrive/include/etherdrive/etherdrive.h 2008-04-28 23:25:10 UTC (rev 217)
@@ -43,6 +43,8 @@
using namespace std;
+class EDMotor;
+
class EtherDrive
{
public:
@@ -63,15 +65,32 @@
// Disable motors
bool motors_off();
+ bool set_control_mode(int8_t m);
+
+ bool set_gain(uint32_t m, char G, int32_t val);
+
+ EDMotor get_motor(uint32_t m);
+
+ void set_drv(uint32_t m, int32_t drv);
+ int32_t get_enc(uint32_t m);
+ int32_t get_cur(uint32_t m);
+ int32_t get_pwm(uint32_t m);
+
// Send an array of motor commands up to 6 in length.
bool drive(size_t num, int32_t* drv);
// Send most recent motor commands, and retrieve update (this must be run at sufficient rate).
bool tick(size_t num = 0, int32_t* enc = 0, int32_t* curr = 0, int32_t* pwm = 0);
+
+
+
private:
bool ready;
int32_t last_drv[6];
+ int32_t last_enc[6];
+ int32_t last_cur[6];
+ int32_t last_pwm[6];
int mot_sock;
int cmd_sock;
@@ -80,5 +99,43 @@
struct sockaddr_in cmd_addr_out;
};
+class EDMotor
+{
+ friend class EtherDrive;
+public:
+ void set_drv(int32_t drv) {
+ driver->set_drv(motor, drv);
+ }
+
+ int32_t get_enc() {
+ return driver->get_enc(motor);
+ }
+
+ int32_t get_cur() {
+ return driver->get_cur(motor);
+ }
+
+ int32_t get_pwm() {
+ return driver->get_pwm(motor);
+ }
+
+ bool set_gains(int32_t P, int32_t I, int32_t D, int32_t W, int32_t M, int32_t Z) {
+ return driver->set_gain(motor, 'P', P) &
+ driver->set_gain(motor, 'I', I) &
+ driver->set_gain(motor, 'D', D) &
+ driver->set_gain(motor, 'W', W) &
+ driver->set_gain(motor, 'M', M) &
+ driver->set_gain(motor, 'Z', Z);
+ }
+protected:
+ EDMotor(EtherDrive* driver, uint32_t motor) : driver(driver), motor(motor) {}
+
+private:
+ EtherDrive* driver;
+
+ uint32_t motor;
+};
+
+
#endif
Deleted: pkg/branches/rosbus/etherdrive/include/etherdrive.h
===================================================================
--- pkg/branches/rosbus/etherdrive/include/etherdrive.h 2008-04-28 23:13:16 UTC (rev 216)
+++ pkg/branches/rosbus/etherdrive/include/etherdrive.h 2008-04-28 23:25:10 UTC (rev 217)
@@ -1,84 +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 ETHERDRIVE_H
-#define ETHERDRIVE_H
-
-#include <sys/types.h>
-#include <sys/socket.h>
-#include <arpa/inet.h>
-
-#include <string>
-
-using namespace std;
-
-class EtherDrive
-{
-public:
- EtherDrive();
- ~EtherDrive();
-
- // Initialize
- bool init(string ip);
-
- void shutdown();
-
- // Manually send EtherDrive command.
- int send_cmd(char* cmd, size_t cmd_len, char* buf, size_t buf_len);
-
- // Enable motors
- bool motors_on();
-
- // Disable motors
- bool motors_off();
-
- // Send an array of motor commands up to 6 in length.
- bool drive(size_t num, int32_t* drv);
-
- // Send most recent motor commands, and retrieve update (this must be run at sufficient rate).
- bool tick(size_t num = 0, int32_t* enc = 0, int32_t* curr = 0, int32_t* pwm = 0);
-private:
- bool ready;
-
- int32_t last_drv[6];
-
- int mot_sock;
- int cmd_sock;
-
- struct sockaddr_in mot_addr_out;
- struct sockaddr_in cmd_addr_out;
-};
-
-#endif
-
Modified: pkg/branches/rosbus/etherdrive/src/etherdrive_control/etherdrive_control.cpp
===================================================================
--- pkg/branches/rosbus/etherdrive/src/etherdrive_control/etherdrive_control.cpp 2008-04-28 23:13:16 UTC (rev 216)
+++ pkg/branches/rosbus/etherdrive/src/etherdrive_control/etherdrive_control.cpp 2008-04-28 23:25:10 UTC (rev 217)
@@ -112,7 +112,7 @@
control_mutex.lock();
- printf("Commanding: %g -- %d\n", last_mot_val[1], tmp_mot_cmd[1]);
+ // printf("Commanding: %g -- %d\n", last_mot_val[1], tmp_mot_cmd[1]);
ed->drive(6,tmp_mot_cmd);
Modified: pkg/branches/rosbus/etherdrive/src/libetherdrive/etherdrive.cpp
===================================================================
--- pkg/branches/rosbus/etherdrive/src/libetherdrive/etherdrive.cpp 2008-04-28 23:13:16 UTC (rev 216)
+++ pkg/branches/rosbus/etherdrive/src/libetherdrive/etherdrive.cpp 2008-04-28 23:25:10 UTC (rev 217)
@@ -48,6 +48,11 @@
}
+EtherDrive::~EtherDrive()
+{
+ shutdown();
+}
+
bool EtherDrive::init(string ip) {
if (ready) {
@@ -94,12 +99,6 @@
}
-EtherDrive::~EtherDrive()
-{
- shutdown();
-}
-
-
void EtherDrive::shutdown() {
if (ready = true) {
@@ -108,10 +107,31 @@
}
ready = false;
-
}
+int EtherDrive::send_cmd(char* cmd, size_t cmd_len, char* buf, size_t buf_len) {
+
+ if (!ready)
+ return -1;
+
+ int n_sent;
+ int n_recv;
+
+ struct sockaddr_in from;
+ socklen_t fromlen = sizeof(struct sockaddr_in);
+
+ n_sent = sendto(cmd_sock, cmd, cmd_len, 0, (struct sockaddr *)&cmd_addr_out, sizeof(mot_addr_out));
+
+ if (n_sent != cmd_len) {
+ return -1;
+ }
+
+ n_recv = recvfrom(cmd_sock, buf, buf_len, 0, (struct sockaddr *)&from, &fromlen);
+
+ return n_recv;
+}
+
bool EtherDrive::motors_on() {
if (ready) {
@@ -119,11 +139,11 @@
cmd[0] = 'm';
cmd[1] = 1;
cmd[2] = 0;
-
+
int32_t res[3];
-
+
int res_len = send_cmd((char*)cmd, 3*sizeof(int32_t), (char*)res, 100*sizeof(int32_t));
-
+
if (res_len == 3) {
if (res[1] == 1) {
return true;
@@ -132,7 +152,6 @@
}
return false;
-
}
bool EtherDrive::motors_off() {
@@ -158,48 +177,96 @@
}
+bool EtherDrive::set_control_mode(int8_t m) {
-int EtherDrive::send_cmd(char* cmd, size_t cmd_len, char* buf, size_t buf_len) {
+ if (ready) {
- int n_sent;
- int n_recv;
+ if (m == 0 || m == 1 || m == 2) {
- struct sockaddr_in from;
- socklen_t fromlen = sizeof(struct sockaddr_in);
+ int32_t cmd[3];
+ cmd[0] = 'i';
+ cmd[1] = m;
+ cmd[2] = 0;
- n_sent = sendto(cmd_sock, cmd, cmd_len, 0, (struct sockaddr *)&cmd_addr_out, sizeof(mot_addr_out));
+ int32_t res[3];
- if (n_sent != cmd_len) {
- return -1;
+ int res_len = send_cmd((char*)cmd, 3*sizeof(int32_t), (char*)res, 100*sizeof(int32_t));
+
+ if (res_len == 3) {
+ if (res[1] == m) {
+ return true;
+ }
+ }
+ }
}
- n_recv = recvfrom(cmd_sock, buf, buf_len, 0, (struct sockaddr *)&from, &fromlen);
+ return false;
+}
- return n_recv;
+bool EtherDrive::set_gain(uint32_t m, char G, int32_t val) {
+ if (ready) {
+
+ if (m < 6) {
+
+ char cmds[] = "PIDWMZ";
+ if ( memchr(cmds, G, strlen(cmds)) != NULL) {
+
+ int32_t cmd[3];
+ cmd[0] = G;
+ cmd[1] = m;
+ cmd[2] = val;
+
+ int32_t res[3];
+
+ int res_len = send_cmd((char*)cmd, 3*sizeof(int32_t), (char*)res, 100*sizeof(int32_t));
+
+ if (res_len == 3) {
+ if (res[1] == m && res[2] == val) {
+ return true;
+ }
+ }
+ }
+ }
+ }
}
-bool EtherDrive::drive(size_t num, int32_t* drv)
-{
- if (num > 6) {
- return false;
+EDMotor EtherDrive::get_motor(uint32_t m) {
+ return EDMotor(this, m);
+}
+
+void EtherDrive::set_drv(uint32_t m, int32_t drv) {
+ if (m < 6) {
+ last_drv[m] = drv;
}
+}
- for (int i = 0; i < num; i++) {
- last_drv[i] = drv[i];
+int32_t EtherDrive::get_enc(uint32_t m) {
+ if (m < 6) {
+ return last_enc[m];
}
+}
- for (int i = num; i < 6; i++) {
- last_drv[i] = 0;
+int32_t EtherDrive::get_cur(uint32_t m) {
+ if (m < 6) {
+ return last_cur[m];
}
+}
- return true;
+int32_t EtherDrive::get_pwm(uint32_t m) {
+ if (m < 6) {
+ return last_pwm[m];
+ }
}
-bool EtherDrive::tick(size_t num, int32_t* enc, int32_t* curr, int32_t* pwm)
+
+bool EtherDrive::tick(size_t num, int32_t* enc, int32_t* cur, int32_t* pwm)
{
+ if (!ready)
+ return false;
+
if (num > 6) {
return false;
}
@@ -224,23 +291,47 @@
return false;
}
+
+ for (int i = 0; i < 6; i++) {
+ last_enc[i] = buf[i];
+ }
+ for (int i = 0; i < 6; i++) {
+ last_cur[i] = buf[6+i];
+ }
+ for (int i = 0; i < 6; i++) {
+ last_pwm[i] = buf[12+i];
+ }
+
+
if (enc > 0) {
for (int i = 0; i < num; i++) {
- enc[i] = buf[i];
+ enc[i] = last_enc[i];
}
}
-
- if (curr > 0) {
+ if (cur > 0) {
for (int i = 0; i < num; i++) {
- curr[i] = buf[6+i];
+ cur[i] = last_cur[i];
}
}
-
if (pwm > 0) {
for (int i = 0; i < num; i++) {
- pwm[i] = buf[12+i];
+ pwm[i] = last_pwm[i];
}
}
return n_recv;
}
+
+bool EtherDrive::drive(size_t num, int32_t* drv)
+{
+ if (num > 6) {
+ return false;
+ }
+
+ for (int i = 0; i < num; i++) {
+ last_drv[i] = drv[i];
+ }
+
+ return true;
+}
+
Modified: pkg/branches/rosbus/etherdrive/test/test_etherdrive/test_etherdrive.cpp
===================================================================
--- pkg/branches/rosbus/etherdrive/test/test_etherdrive/test_etherdrive.cpp 2008-04-28 23:13:16 UTC (rev 216)
+++ pkg/branches/rosbus/etherdrive/test/test_etherdrive/test_etherdrive.cpp 2008-04-28 23:25:10 UTC (rev 217)
@@ -57,7 +57,7 @@
if (!e.drive(1,&drv))
printf("Drive problem!.");
if (!e.tick(2,enc,cur))
-// // // printf("Tick problem!.");
+ printf("Tick problem!.");
printf("Encoder0: %d Current: %d\n", enc[0], cur[0]);
printf("Encoder1: %d Current: %d\n", enc[1], cur[1]);
Copied: pkg/branches/rosbus/etherdrive/test/test_etherdrive2 (from rev 216, pkg/trunk/etherdrive/test/test_etherdrive2)
Deleted: pkg/branches/rosbus/etherdrive/test/test_etherdrive2/Makefile
===================================================================
--- pkg/trunk/etherdrive/test/test_etherdrive2/Makefile 2008-04-28 23:13:16 UTC (rev 216)
+++ pkg/branches/rosbus/etherdrive/test/test_etherdrive2/Makefile 2008-04-28 23:25:10 UTC (rev 217)
@@ -1,4 +0,0 @@
-SRC = test_etherdrive2.cpp
-OUT = test_etherdrive2
-PKG = etherdrive
-include $(shell $(ROS_ROOT)/rospack find roscpp)/make_include/node.mk
Copied: pkg/branches/rosbus/etherdrive/test/test_etherdrive2/Makefile (from rev 216, pkg/trunk/etherdrive/test/test_etherdrive2/Makefile)
===================================================================
--- pkg/branches/rosbus/etherdrive/test/test_etherdrive2/Makefile (rev 0)
+++ pkg/branches/rosbus/etherdrive/test/test_etherdrive2/Makefile 2008-04-28 23:25:10 UTC (rev 217)
@@ -0,0 +1,4 @@
+SRC = test_etherdrive2.cpp
+OUT = test_etherdrive2
+PKG = etherdrive
+include $(shell $(ROS_ROOT)/rospack find roscpp)/make_include/node.mk
Deleted: pkg/branches/rosbus/etherdrive/test/test_etherdrive2/test_etherdrive2.cpp
===================================================================
--- pkg/trunk/etherdrive/test/test_etherdrive2/test_etherdrive2.cpp 2008-04-28 23:13:16 UTC (rev 216)
+++ pkg/branches/rosbus/etherdrive/test/test_etherdrive2/test_etherdrive2.cpp 2008-04-28 23:25:10 UTC (rev 217)
@@ -1,92 +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.
-*********************************************************************/
-
-#include "etherdrive/etherdrive.h"
-#include <iostream>
-
-using namespace std;
-
-int main() {
-
- EtherDrive e;
-
- if (!e.init("192.168.0.100")) {
- cout << "Could not initialize etherdrive." << endl;
- return -1;
- }
-
- EDMotor m0 = e.get_motor(0);
- EDMotor m1 = e.get_motor(1);
-
- e.motors_on();
-
- int count = 0;
-
- int drv = 100000;
-
- e.set_control_mode(2);
-
- if (!m1.set_gains(150, 20, -10, 50, 200, 13)) {
- printf("Setting gains failed!\n");
- return 0;
- }
-
- while (1) {
-
- m0.set_drv(drv);
-
- if (!e.tick())
- printf("Tick problem!.");
-
- printf("Encoder0: %d Current: %d PWM: %d\n", m0.get_enc(), m0.get_cur(), m0.get_pwm());
- printf("Encoder1: %d Current: %d PWM: %d\n", m1.get_enc(), m1.get_cur(), m1.get_pwm());
-
-
- // Crappy control
- if (abs(m0.get_cur()) > 200) {
- if (count++ > 50)
- e.motors_off();
- } else {
- count = 0;
- }
-
- if (m0.get_enc() == 100000) {
- drv = -100000;
- } else if (m0.get_enc() == -100000) {
- drv = 100000;
- }
- }
-
- e.shutdown();
-}
Copied: pkg/branches/rosbus/etherdrive/test/test_etherdrive2/test_etherdrive2.cpp (from rev 216, pkg/trunk/etherdrive/test/test_etherdrive2/test_etherdrive2.cpp)
===================================================================
--- pkg/branches/rosbus/etherdrive/test/test_etherdrive2/test_etherdrive2.cpp (rev 0)
+++ pkg/branches/rosbus/etherdrive/test/test_etherdrive2/test_etherdrive2.cpp 2008-04-28 23:25:10 UTC (rev 217)
@@ -0,0 +1,92 @@
+/*********************************************************************
+* 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 "etherdrive/etherdrive.h"
+#include <iostream>
+
+using namespace std;
+
+int main() {
+
+ EtherDrive e;
+
+ if (!e.init("192.168.0.100")) {
+ cout << "Could not initialize etherdrive." << endl;
+ return -1;
+ }
+
+ EDMotor m0 = e.get_motor(0);
+ EDMotor m1 = e.get_motor(1);
+
+ e.motors_on();
+
+ int count = 0;
+
+ int drv = 100000;
+
+ e.set_control_mode(2);
+
+ if (!m1.set_gains(150, 20, -10, 50, 200, 13)) {
+ printf("Setting gains failed!\n");
+ return 0;
+ }
+
+ while (1) {
+
+ m0.set_drv(drv);
+
+ if (!e.tick())
+ printf("Tick problem!.");
+
+ printf("Encoder0: %d Current: %d PWM: %d\n", m0.get_enc(), m0.get_cur(), m0.get_pwm());
+ printf("Encoder1: %d Current: %d PWM: %d\n", m1.get_enc(), m1.get_cur(), m1.get_pwm());
+
+
+ // Crappy control
+ if (abs(m0.get_cur()) > 200) {
+ if (count++ > 50)
+ e.motors_off();
+ } else {
+ count = 0;
+ }
+
+ if (m0.get_enc() == 100000) {
+ drv = -100000;
+ } else if (m0.get_enc() == -100000) {
+ drv = 100000;
+ }
+ }
+
+ e.shutdown();
+}
Copied: pkg/branches/rosbus/kdtree (from rev 216, pkg/trunk/kdtree)
Deleted: pkg/branches/rosbus/kdtree/Makefile
===================================================================
--- pkg/trunk/kdtree/Makefile 2008-04-28 23:13:16 UTC (rev 216)
+++ pkg/branches/rosbus/kdtree/Makefile 2008-04-28 23:25:10 UTC (rev 217)
@@ -1,4 +0,0 @@
-SRC = kdtree2.31/src-c++/kdtree2.cpp
-OUT = lib/libkdtree.a
-PKG = kdtree
-include $(shell $(ROS_ROOT)/rospack find roscpp)/make_include/lib.mk
Copied: pkg/branches/rosbus/kdtree/Makefile (from rev 216, pkg/trunk/kdtree/Makefile)
===================================================================
--- pkg/branches/rosbus/kdtree/Makefile (rev 0)
+++ pkg/branches/rosbus/kdtree/Makefile 2008-04-28 23:25:10 UTC (rev 217)
@@ -0,0 +1,4 @@
+SRC = kdtree2.31/src-c++/kdtree2.cpp
+OUT = lib/libkdtree.a
+PKG = kdtree
+include $(shell $(ROS_ROOT)/rospack find roscpp)/make_include/lib.mk
Copied: pkg/branches/rosbus/kdtree/include (from rev 216, pkg/trunk/kdtree/include)
Copied: pkg/branches/rosbus/kdtree/include/kdtree (from rev 216, pkg/trunk/kdtree/include/kdtree)
Deleted: pkg/branches/rosbus/kdtree/kdtree2.tar.gz
===================================================================
(Binary files differ)
Copied: pkg/branches/rosbus/kdtree/kdtree2.tar.gz (from rev 216, pkg/trunk/kdtree/kdtree2.tar.gz)
===================================================================
(Binary files differ)
Copied: pkg/branches/rosbus/kdtree/lib (from rev 216, pkg/trunk/kdtree/lib)
Deleted: pkg/branches/rosbus/kdtree/manifest.xml
===================================================================
--- pkg/trunk/kdtree/manifest.xml 2008-04-28 23:13:16 UTC (rev 216)
+++ pkg/branches/rosbus/kdtree/manifest.xml 2008-04-28 23:25:10 UTC (rev 217)
@@ -1,9 +0,0 @@
-<package>
-<description brief="KDTree library">
-A Wrapper for the KDTREE2 library.
-</description>
-<author>Matthew B. Kennel</author>
-<license>Academic Free License 1.1</license>
-<url>http://arxiv.org/abs/physics/0408067</url>
-</package>
-
Copied: pkg/branches/rosbus/kdtree/manifest.xml (from rev 216, pkg/trunk/kdtree/manifest.xml)
===================================================================
--- pkg/branches/rosbus/kdtree/manifest.xml (rev 0)
+++ pkg/branches/rosbus/kdtree/manifest.xml 2008-04-28 23:25:10 UTC (rev 217)
@@ -0,0 +1,9 @@
+<package>
+<description brief="KDTree library">
+A Wrapper for the KDTREE2 library.
+</description>
+<author>Matthew B. Kennel</author>
+<license>Academic Free License 1.1</license>
+<url>http://arxiv.org/abs/physics/0408067</url>
+</package>
+
Deleted: pkg/branches/rosbus/kdtree/rosbuild
===================================================================
--- pkg/trunk/kdtree/rosbuild 2008-04-28 23:13:16 UTC (rev 216)
+++ pkg/branches/rosbus/kdtree/rosbuild 2008-04-28 23:25:10 UTC (rev 217)
@@ -1,36 +0,0 @@
-#!/usr/bin/env ruby
-require 'fileutils.rb'
-
-pkg_path = File.expand_path($0).split('/')
-pkg_path = pkg_path[0,pkg_path.length-1].join('/')
-puts "package path: [#{pkg_path}]"
-if pkg_path.length < 1
- puts "woah! package path looks scary. something isn't right. bailing..."
- exit
-end
-
-Dir.chdir(pkg_path)
-# TODO make sure we got there
-
-if ARGV.length == 0 || ARGV[0] == 'update'
- puts "extracting KDTree ============================"
- system("tar xzvf kdtree2.tar.gz")
- system("cp kdtree2.31/src-c++/kdtree2.hpp include/kdtree/")
- puts "making kdtree2 ================================="
- system("make")
- puts "building test ================================="
- Dir.chdir "#{pkg_path}/test"
- system("make")
- puts "done! hooray! =================================="
-elsif ARGV[0] == 'clean'
- Dir.chdir "#{pkg_path}"
- system("make clean")
- system("rm -rf kdtree2.31")
- system("rm include/kdtree/kdtree2.hpp")
- Dir.chdir "#{pkg_path}/test"
- system("make clean")
- puts "done!"
-else
- puts "unknown parameter: #{ARGV[0]}"
-end
-
Copied: pkg/branches/rosbus/kdtree/rosbuild (from rev 216, pkg/trunk/kdtree/rosbuild)
===================================================================
--- pkg/branches/rosbus/kdtree/rosbuild (rev 0)
+++ pkg/branches/rosbus/kdtree/rosbuild 2008-04-28 23:25:10 UTC (rev 217)
@@ -0,0 +1,36 @@
+#!/usr/bin/env ruby
+require 'fileutils.rb'
+
+pkg_path = File.expand_path($0).split('/')
+pkg_path = pkg_path[0,pkg_path.length-1].join('/')
+puts "package path: [#{pkg_path}]"
+if pkg_path.length < 1
+ puts "woah! package path looks scary. something isn't right. bailing..."
+ exit
+end
+
+Dir.chdir(pkg_path)
+# TODO make sure we got there
+
+if ARGV.length == 0 || ARGV[0] == 'update'
+ puts "extracting KDTree ============================"
+ system("tar xzvf kdtree2.tar.gz")
+ system("cp kdtree2.31/src-c++/kdtree2.hpp include/kdtree/")
+ puts "making kdtree2 ================================="
+ system("make")
+ puts "building test ================================="
+ Dir.chdir "#{pkg_path}/test"
+ system("make")
+ puts "done! hooray! =================================="
+elsif ARGV[0] == 'clean'
+ Dir.chdir "#{pkg_path}"
+ system("make clean")
+ system("rm -rf kdtree2.31")
+ system("rm include/kdtree/kdtree2.hpp")
+ Dir.chdir "#{pkg_path}/test"
+ system("make clean")
+ puts "done!"
+else
+ puts "unknown parameter: #{ARGV[0]}"
+end
+
Copied: pkg/branches/rosbus/kdtree/test (from rev 216, pkg/trunk/kdtree/test)
Deleted: pkg/branches/rosbus/kdtree/test/Makefile
===================================================================
--- pkg/trunk/kdtree/test/Makefile 2008-04-28 23:13:16 UTC (rev 216)
+++ pkg/branches/rosbus/kdtree/test/Makefile 2008-04-28 23:25:10 UTC (rev 217)
@@ -1,4 +0,0 @@
-SRC = test.cpp
-OUT = test
-PKG = kdtree
-include $(shell $(ROS_ROOT)/rospack find roscpp)/make_include/node.mk
Copied: pkg/branches/rosbus/kdtree/test/Makefile (from rev 216, pkg/trunk/kdtree/test/Makefile)
===================================================================
--- pkg/branches/rosbus/kdtree/test/Makefile (rev 0)
+++ pkg/branches/rosbus/kdtree/test/Makefile 2008-04-28 23:25:10 UTC (rev 217)
@@ -0,0 +1,4 @@
+SRC = test.cpp
+OUT = test
+PKG = kdtree
+include $(shell $(ROS_ROOT)/rospack find roscpp)/make_include/node.mk
Deleted: pkg/branches/rosbus/kdtree/test/test.cpp
===================================================================
--- pkg/trunk/kdtree/test/test.cpp 2008-04-28 23:13:16 UTC (rev 216)
+++ pkg/branches/rosbus/kdtree/test/test.cpp 2008-04-28 23:25:10 UTC (rev 217)
@@ -1,78 +0,0 @@
-#include "kdtree/kdtree2.hpp"
-
-#include <boost/multi_array.hpp>
-#include <boost/random.hpp>
-#include <sys/time.h>
-
-using namespace boost;
-
- double timeval_diff (struct timeval x,
- struct timeval y) {
- double dsec = x.tv_sec - y.tv_sec;
- double dusec = x.tv_usec - y.tv_usec;
- return dsec + dusec/1000000.0;
- }
-
-static minstd_rand generator(42u);
-static uniform_real<> uni_dist(0,1);
-variate_generator<minstd_rand&,uniform_real<> > uni(generator,uni_dist);
-
-float random_variate() {
- // between [0,1)
- return(uni());
-}
-
-//
-// define, for convenience a 2d array of floats.
-//
-typedef multi_array<float,2> array2dfloat;
-
-int main() {
- array2dfloat realdata;
-
- kdtree2* tree;
-
- int N = 1000000;
- int dim = 3;
-
- struct timeval t0, t1;
-
- realdata.resize(extents[N][dim]);
-
- for (int i=0; i<N; i++) {
- for (int j=0; j<dim; j++)
- realdata[i][j] = random_variate();
- }
-
- kdtree2_result_vector result, resultbrute;
-
- cout << "Building tree... ";
-
- gettimeofday(&t0, NULL);
-
- tree = new kdtree2(realdata,true);
- tree->sort_results = false;
-
- gettimeofday(&t1, NULL);
- cout << timeval_diff(t1, t0) << endl;
-
- int nn = 1;
-
- // tree->n_nearest_around_point(150000,1,nn,result);
-
- cout << "Finding nearest... ";
-
- gettimeofday(&t0, NULL);
- for (int k = 0; k < N;k++) {
- tree->n_nearest_around_point(k,1,nn,result);
- }
- gettimeofday(&t1, NULL);
- cout << timeval_diff(t1, t0) << endl;
-
- /*
- for (int k=0; k<nn; k++) {
- cout << "Result " << k << ", " << result[k].dis << ", " << result[k].idx << endl;
- }
- */
-}
-
Copied: pkg/branches/rosbus/kdtree/test/test.cpp (from rev 216, pkg/trunk/kdtree/test/test.cpp)
===================================================================
--- pkg/branches/rosbus/kdtree/test/test.cpp (rev 0)
+++ pkg/branches/rosbus/kdtree/test/test.cpp 2008-04-28 23:25:10 UTC (rev 217)
@@ -0,0 +1,78 @@
+#include "kdtree/kdtree2.hpp"
+
+#include <boost/multi_array.hpp>
+#include <boost/random.hpp>
+#include <sys/time.h>
+
+using namespace boost;
+
+ double timeval_diff (struct timeval x,
+ struct timeval y) {
+ double dsec = x.tv_sec - y.tv_sec;
+ double dusec = x.tv_usec - y.tv_usec;
+ return dsec + dusec/1000000.0;
+ }
+
+static minstd_rand generator(42u);
+static uniform_real<> uni_dist(0,1);
+variate_generator<minstd_rand&,uniform_real<> > uni(generator,uni_dist);
+
+float random_variate() {
+ // between [0,1)
+ return(uni());
+}
+
+//
+// define, for convenience a 2d array of floats.
+//
+typedef multi_array<float,2> array2dfloat;
+
+int main() {
+ array2dfloat realdata;
+
+ kdtree2* tree;
+
+ int N = 1000000;
+ int dim = 3;
+
+ struct timeval t0, t1;
+
+ realdata.resize(extents[N][dim]);
+
+ for (int i=0; i<N; i++) {
+ for (int j=0; j<dim; j++)
+ realdata[i][j] = random_variate();
+ }
+
+ kdtree2_result_vector result, resultbrute;
+
+ cout << "Building tree... ";
+
+ gettimeofday(&t0, NULL);
+
+ tree = new kdtree2(realdata,true);
+ tree->sort_results = false;
+
+ gettimeofday(&t1, NULL);
+ cout << timeval_diff(t1, t0) << endl;
+
+ int nn = 1;
+
+ // tree->n_nearest_around_point(150000,1,nn,result);
+
+ cout << "Finding nearest... ";
+
+ gettimeofday(&t0, NULL);
+ for (int k = 0; k < N;k++) {
+ tree->n_nearest_around_point(k,1,nn,result);
+ }
+ gettimeofday(&t1, NULL);
+ cout << timeval_diff(t1, t0) << endl;
+
+ /*
+ for (int k=0; k<nn; k++) {
+ cout << "Result " << k << ", " << result[k].dis << ", " << result[k].idx << endl;
+ }
+ */
+}
+
Copied: pkg/branches/rosbus/libTF/doc (from rev 216, pkg/trunk/libTF/doc)
Deleted: pkg/branches/rosbus/libTF/doc/Makefile
===================================================================
--- pkg/trunk/libTF/doc/Makefile 2008-04-28 23:13:16 UTC (rev 216)
+++ pkg/branches/rosbus/libTF/doc/Makefile 2008-04-28 23:25:10 UTC (rev 217)
@@ -1,13 +0,0 @@
-all: manual
-
-
-manual: libTF_Manual.tex
- pdflatex $^
- bibtex libTF_Manual
- makeindex libTF_Manual.idx
- pdflatex $^
- pdflatex $^
-
-clean:
- rm -f *~ *.aux *.toc *.log *.ilg *.ind *.idx *.out *.bbl *.blg
- rm -f libTF_Manual.pdf
\ No newline at end of file
Copied: pkg/branches/rosbus/libTF/doc/Makefile (from rev 216, pkg/trunk/libTF/doc/Makefile)
===================================================================
--- pkg/branches/rosbus/libTF/doc/Makefile (rev 0)
+++ pkg/branches/rosbus/libTF/doc/Makefile 2008-04-28 23:25:10 UTC (rev 217)
@@ -0,0 +1,13 @@
+all: manual
+
+
+manual: libTF_Manual.tex
+ pdflatex $^
+ bibtex libTF_Manual
+ makeindex libTF_Manual.idx
+ pdflatex $^
+ pdflatex $^
+
+clean:
+ rm -f *~ *.aux *.toc *.log *.ilg *.ind *.idx *.out *.bbl *.blg
+ rm -f libTF_Manual.pdf
\ No newline at end of file
Deleted: pkg/branches/rosbus/libTF/doc/README.txt
===================================================================
--- pkg/trunk/libTF/doc/README.txt 2008-04-28 23:13:16 UTC (rev 216)
+++ pkg/branches/rosbus/libTF/doc/README.txt 2008-04-28 23:25:10 UTC (rev 217)
@@ -1,8 +0,0 @@
-*************
-*** libTF ***
-*************
-
-
-
-A transform library and ROS wrapper to keep track of coordinate transforms for the whole system.
-
Copied: pkg/branches/rosbus/libTF/doc/README.txt (from rev 216, pkg/trunk/libTF/doc/README.txt)
===================================================================
--- pkg/branches/rosbus/libTF/doc/README.txt (rev 0)
+++ pkg/branches/rosbus/libTF/doc/README.txt 2008-04-28 23:25:10 UTC (rev 217)
@@ -0,0 +1,8 @@
+*************
+*** libTF ***
+*************
+
+
+
+A transform library and ROS wrapper to keep track of coordinate transforms for the whole system.
+
Deleted: pkg/branches/rosbus/libTF/doc/libTF_Manual.bib
===================================================================
--- pkg/trunk/libTF/doc/libTF_Manual.bib 2008-04-28 23:13:16 UTC (rev 216)
+++ pkg/branches/rosbus/libTF/doc/libTF_Manual.bib 2008-04-28 23:25:10 UTC (rev 217)
@@ -1,13 +0,0 @@
-@article{SHOEMAKE,
- author = {Ken Shoemake},
- title = {Animating rotation with quaternion curves},
- journal = {SIGGRAPH Comput. Graph.},
- volume = {19},
- number = {3},
- year = {1985},
- issn = {0097-8930},
- pages = {245--254},
- doi = {http://doi.acm.org/10.1145/325165.325242},
- publisher = {ACM},
- address = {New York, NY, USA},
- }
Copied: pkg/branches/rosbus/libTF/doc/libTF_Manual.bib (from rev 216, pkg/trunk/libTF/doc/libTF_Manual.bib)
===================================================================
--- pkg/branches/rosbus/libTF/doc/libTF_Manual.bib (rev 0)
+++ pkg/branches/rosbus/libTF/doc/libTF_Manual.bib 2008-04-28 23:25:10 UTC (rev 217)
@@ -0,0 +1,13 @@
+@article{SHOEMAKE,
+ author = {Ken Shoemake},
+ title = {Animating rotation with quaternion curves},
+ journal = {SIGGRAPH Comput. Graph.},
+ volume = {19},
+ number = {3},
+ year = {1985},
+ issn = {0097-8930},
+ pages = {245--254},
+ doi = {http://doi.acm.org/10.1145/325165.325242},
+ publisher = {ACM},
+ address = {New York, NY, USA},
+ }
Deleted: pkg/branches/rosbus/libTF/doc/libTF_Manual.tex
===================================================================
--- pkg/trunk/libTF/doc/libTF_Manual.tex 2008-04-28 23:13:16 UTC (rev 216)
+++ pkg/branches/rosbus/libTF/doc/libTF_Manual.tex 2008-04-28 23:25:10 UTC (rev 217)
@@ -1,338 +0,0 @@
-\documentclass[12pt]{article}
-
-\usepackage{makeidx}
-\usepackage{hyperref}
-\usepackage{amsmath}
-\usepackage{float}
-\makeindex
-
-% a todo macro
-\newcommand{\todo}[1]{\vspace{3 mm}\par \noindent {\textsc{ToDo}}\framebox{
-\begin{minipage}[c]{1.0\hsize}\tt #1 \end{minipage}}\vspace{3mm}\par}
-
-\floatstyle{ruled}
-\newfloat{program}{thp}{lop}
-\floatname{program}{Program}
-
-\newfloat{struct}{thp}{lop}
-\floatname{struct}{Data Structure}
-
-
-\begin{document}
-\title{libTF Manual}
-\author{Tully Foote\\
-\href{mailto:tf...@wi...}{\texttt{tfoote at willowgarage.com}}}
-\date{\today}
-\maketitle
-
-\tableofcontents
-\pagebreak
-
-\section{Overview}
-litTF is designed to provide a simple interface for keeping track
-of coordinate transforms within a robotic framework.
-A simple case which was one of the driving design considerations was
-the use of a sensor on an actuated platform. In this case the sensor
-will report data in the frame of the moving platform, but the data is much
-more useful in the world frame. To provide the transform libTF will be told
-the position of the platform periodically as it moves. And when prompted libTF can
-provide the transform from the sensor frame to the world frame at the time when the
-sensor data was acquired. Not only will libTF will keep track of the transform between
-two coordinate frames, but it will also keep track of a whole tree of coordinate frames,
-and automatically chain transformations together between any two connected frames.
-
-\section{Transform Library}
-
-\subsection{Terminology and Conventions}
-\paragraph {Coodinate Frame}
-\index{Coordinate Frame}
-In this documentation the default
-will be a right handed system with X forward, Y left and Z up.
-
-\paragraph {Denavit-Hartenberg Parameters (DH Parameters)}
-\index{Denavit-Hartenberg Parameters}
-DH Parameters are a way to concicely represent a rigid body tranformation in three dimentions.
-It has four parameters: length, twist, offset, and angle. In addition to using the optimal
-amount of data to store the transformation, there are also optimized methods of chaining
-transformations together. And lastly the parameters can directly represent rotary and prismatic
-joints found on robotic arms.
-See \url{http://en.wikipedia.org/wiki/Denavit-Hartenberg_Parameters} for more details.
-
-\todo{reproduce labeled diagram a good example \url{http://uwf.edu/ria/robotics/robotdraw/DH_parm.htm}}
-
-\paragraph {Euler Angles}
-\index{Euler Angles}
-For this library Euler angles are considered to be translations in x, y, z, followed by a rotation around z, y, x.
-With the respective angular changes referred to as yaw, pitch and roll.
-
-\paragraph {Homogeneous Transformation Matrix}
-\index{Homogeneous Transform}
-Homogeneous Transformation Matrices are a simple way to manipulate 3D translations and rotations
-with standard matrix multiplication. It is a composite of a standard 3x3 rotation matrix
-(see \url{http://en.wikipedia.org/wiki/Rotation_matrix}) and a translation vector.
-
-Let $_1R_0$ be the 3x3 rotation matrix defined by the Euler angles $(yaw_0,pitch_0,roll_0)$
-and let $_1T_0$ be the column vector $(x_0,y_0,z_0)^T$ representing the translation. The combination
-of these two transformations results in the transformation of reference frame 0 to reference frame 1.
-
-A point P in frame 1, $P_1$, can be transformed into frame 1, $P_0$, by the following:
-
-\begin{equation}
-\label{eqn:HT}
-\begin{pmatrix}
-P_0 \\
-1
-\end{pmatrix}
-=
-\begin{pmatrix}
-_0R_1 & _0T_1 \\
-1 & 1
-\end{pmatrix}
-\begin{pmatrix}
-P_1 \\
-1
-\end{pmatrix}
-\end{equation}
-
-The net result is a 4x4 transformation matrix which does both the rotation and translation
-between coordinate frames. The basic approach is to use a 4x1 vector consisting of $(x,y,z,1)^T_1$
-and by left multiplying by $_0A_1$ it will result in $(x',y',z',1)^T_1$.
-
-The matrix library used within this library is Newmat10.
-
-
-\paragraph {Newmat10}
-\index{Newmat10}
-Newmat10 is the matrix library used in this library. Documentation for Newmat can be found at
-\url{http://www.robertnz.net/nm10.htm}.
-
-\subsection{libTF API}
-\index{libTF API}
-The class which provides external functionality is named TransformReference.
-
-\subsubsection{Data Types}
-\begin{verbatim}
-typedef unsigned long long ULLtime;
-\end{verbatim}
-
-\texttt{ULLtime} provides a simple typedef for the timestamp used in this library.
-\texttt{ULLtime} is the number of nanoseconds since the epoch(1970) expressed as
-an unsigned long long, which should be equivilant to a \texttt{uint64\_t}.
-
-\subsubsection{Constructor}
-\index{libTF API!Constructor}
-\begin{verbatim}
-TransformReference(ULLtime cache_time = DEFAULT_CACHE_TIME);
-\end{verbatim}
-This is the constructor for the class. It's optional argument is
-how long to keep a history of the transforms. It is expressed in
-\texttt{ULLtime}, nanoseconds since the epoch(1970).
-
-\subsubsection{Mutators}
-
-\paragraph{setWithEulers}
-A method to set the parameters of a coordinate transform with Euler angles. \index{libTF API!setWithEulers}
-\begin{verbatim}
-void setWithEulers(unsigned int framid, unsigned int parentid,
- double x, double y, double z,
- double yaw, double pitch, double roll,
- unsigned long long time);
-\end{verbatim}
-
-\paragraph{setWithDH}
-A method to set the parameters of a coordinate transform using DH Parameters. \index{libTF API!setWithDH}
-\begin{verbatim}
-void setWithDH(unsigned int framid, unsigned int parentid,
- double length, double alpha,
- double offset, double theta,
- unsigned long long time);
-\end{verbatim}
-
-
-
-\paragraph{setWithMatrix}
-A method to set the parameters of a coordinate transform with a homogeneous transformation matrix. \index{libTF API!setWithMatrix}
-\begin{verbatim}
-unimplemented
-\end{verbatim}
-
-\paragraph{setWithQuaternion}
-A method to set the parameters of a coordinate transform with Quaternions. \index{libTF API!setWithQuaternion}
-\begin{verbatim}
-unimplemented
-\end{verbatim}
-
-
-\subsubsection{Accessors}
-\paragraph{getMatrix}
-\index{libTF API!getMatrix}
-\begin{verbatim}
-NEWMAT::Matrix getMatrix(unsigned int target_frame,
- unsigned int source_frame,
- unsigned long long time);
-\end{verbatim}
-
-\paragraph{viewChain}
-\index{libTF API!viewChain}
-\begin{verbatim}
-std::string viewChain(unsigned int target_frame,
- unsigned int source_frame);
-\end{verbatim}
-
-\section{ROS Integration}
-
-unimplemented
-\subsection{libTF ROS API}
-unimplemented
-
-\section{Example Usage}
-\subsection{Library}
-%\begin{program}[H]
-\begin{verbatim}
-#include ``libTF/libTF.h''
-#include <time.h>
-
-using namespace std;
-
-int main(void)
-{
- double dx,dy,dz,dyaw,dp,dr;
- TransformReference mTR;
-
- //Temporary Variables
- dx = dy= dz = 0;
- dyaw = dp = dr = 0.1;
-
- unsigned long long atime = mTR.gettime();
-
-
- //Fill in some transforms
- // mTR.setWithEulers(10,2,1,1,1,dyaw,dp,dr,atime); //Switching out for DH par
-ams below
- mTR.setWithDH(10,2,1.0,1.0,1.0,dyaw,atime);
- //mTR.setWithEulers(2,3,1-1,1,1,dyaw,dp,dr,atime-1000);
- mTR.setWithEulers(2,3,1,1,1,dyaw,dp,dr,atime-100);
- mTR.setWithEulers(2,3,1,1,1,dyaw,dp,dr,atime-50);
- mTR.setWithEulers(2,3,1,1,1,dyaw,dp,dr,atime-1000);
- //mTR.setWithEulers(2,3,1+1,1,1,dyaw,dp,dr,atime+1000);
- mTR.setWithEulers(3,5,dx,dy,dz,dyaw,dp,dr,atime);
- mTR.setWithEulers(5,1,dx,dy,dz,dyaw,dp,dr,atime);
- mTR.setWithEulers(6,5,dx,dy,dz,dyaw,dp,dr,atime);
- mTR.setWithEulers(6,5,dx,dy,dz,dyaw,dp,dr,atime);
- mTR.setWithEulers(7,6,1,1,1,dyaw,dp,dr,atime);
- mTR.setWithDH(8,7,1.0,1.0,1.0,dyaw,atime);
- //mTR.setWithEulers(8,7,1,1,1,dyaw,dp,dr,atime); //Switching out for DH params
- above
-
-
- //Demonstrate InvalidFrame LookupException
- try
- {
- std::cout<< mTR.viewChain(10,9);
- }
- catch (TransformReference::LookupException &ex)
- {
- std::cout << ``Caught `` << ex.what()<<std::endl;
- }
-
-
- // See the list of transforms to get between the frames
- std::cout<<''Viewing (10,8):''<<std::endl;
- std::cout << mTR.viewChain(10,8);
-
-
- //See the resultant transform
- std::cout <<''Calling getMatrix(10,8)''<<std::endl;
- NEWMAT::Matrix mat = mTR.getMatrix(10,8,atime);
- std::cout << ``Result of getMatrix(10,8,atime):'' << std::endl << mat<< std::end
-l;
-
- //Break the graph, making it loop and demonstrate catching MaxDepthException
- mTR.setWithEulers(6,7,dx,dy,dz,dyaw,dp,dr,atime);
-
- try {
- std::cout<<mTR.viewChain(10,8);
- }
- catch (TransformReference::MaxDepthException &ex)
- {
- std::cout <<''caught loop in graph''<<std::endl;
- }
-
- //Break the graph, making it disconnected, and demonstrate catching Connectivi
-tyException
- mTR.setWithEulers(6,0,dx,dy,dz,dyaw,dp,dr,atime);
-
- try {
- std::cout<<mTR.viewChain(10,8);
- }
- catch (TransformReference::ConnectivityException &ex)
- {
- std::cout <<''caught unconnected frame''<<std::endl;
- }
- return 0;
-};
-
-\end{verbatim}
-%\end{program}
-
-\subsection{ROS Implementation}
-\begin{verbatim}
-TODO
-\end{verbatim}
-
-\section{Summary of Internal Mathematics}
-This libarary uses quaternion notation as the internal representation
-of coordinate transforms. Transform information is stored in a linked
-list sorted by time. When a transform is requested the closest two points
-on the linked list are found and then interpolated in time to generate the
-return value.
-
-\subsection{Storage}
-The internal storage for the transforms consists of:
-\begin{struct}[H]
-\caption{libTF Internal Data Storage}
-\begin{verbatim}
- double xtranslation;// The three components of translation
- double ytranslation;
- double ztranslation;
- double xrotation; // The three components of the rotation axis
- double yrotation;
- double zrotation;
- double w; // Omega
- unsigned long long time; //nano seconds since 1970
-\end{verbatim}
-\end{struct}
-
-
-\subsection{Interpolation}
-The interpolation method used in this library is Spherical Linear Interpolation
-(SLERP). \index{SLERP} The standard formula for SLERP is defined in Equation \ref{eq:slerp}.
-The inputs are points $p_0$ and $p_1$, and $t$ is the proportion to interpolate
-between $p_0$ and $p_1$, and $\Omega$ is the angle between the axis of the two quaternions.
-
-\begin{equation}
-Slerp(p_0,p_1;t) = \frac{sin((1-t)\Omega)}{sin(\Omega)} * p_0 + \frac{sin(t*\Omega)}{sin(\Omega)} * p_1
-\label{eq:slerp}
-\end{equation}
-
-\todo{add graphic}
-
-\paragraph{Alternatives}
-The SLERP technique was developed in 1985 by Ken Shoemake. \cite{SHOEMAKE} SLERP has been
-largely adopted in the field of computer graphics, however there have been many alternatives
-developed. The advantages of SLERP are that it provides a constant speed solution
-along the shortest path over the 4D unit sphere. This is not optimal in all cases however.
-A good discussion of various alternatives is here \url{http://number-none.com/product/Understanding%20Slerp,%20Then%20Not%20Using%20It/index.html}.
-The approaches discussed above are normalized linear interpolation (nlerp), and
-log-quaternion linear interpolation (log-quaternion lerp).
-SLERP can be recursively called to do a cubic interpolation, called SQUAD for short.
-This is discussed at \url{http://www.sjbrown.co.uk/?article=quaternions}.
-
-
-
-\bibliographystyle{plain}
-\bibliography{libTF_Manual}
-
-\printindex
-\end{document}
-
Copied: pkg/branches/rosbus/libTF/doc/libTF_Manual.tex (from rev 216, pkg/trunk/libTF/doc/libTF_Manual.tex)
===================================================================
--- pkg/branches/rosbus/libTF/doc/libTF_Manual.tex (rev 0)
+++ pkg/branches/rosbus/libTF/doc/libTF_Manual.tex 2008-04-28 23:25:10 UTC (rev 217)
@@ -0,0 +1,338 @@
+\documentclass[12pt]{article}
+
+\usepackage{makeidx}
+\usepackage{hyperref}
+\usepackage{amsmath}
+\usepackage{float}
+\makeindex
+
+% a todo macro
+\newcommand{\todo}[1]{\vspace{3 mm}\par \noindent {\textsc{ToDo}}\framebox{
+\begin{minipage}[c]{1.0\hsize}\tt #1 \end{minipage}}\vspace{3mm}\par}
+
+\floatstyle{ruled}
+\newfloat{program}{thp}{lop}
+\floatname{program}{Program}
+
+\newfloat{struct}{thp}{lop}
+\floatname{struct}{Data Structure}
+
+
+\begin{document}
+\title{libTF Manual}
+\author{Tully Foote\\
+\href{mailto:tf...@wi...}{\texttt{tfoote at willowgarage.com}}}
+\date{\today}
+\maketitle
+
+\tableofcontents
+\pagebreak
+
+\section{Overview}
+litTF is designed to provide a simple interface for keeping track
+of coordinate transforms within a robotic framework.
+A simple case which was one of the driving design considerations was
+the use of a sensor on an actuated platform. In this case the sensor
+will report data in the frame of the moving platform, but the data is much
+more useful in the world frame. To provide the transform libTF will be told
+the position of the platform periodically as it moves. And when prompted libTF can
+provide the transform from the sensor frame to the world frame at the time when the
+sensor data was acquired. Not only will libTF will keep track of the transform between
+two coordinate frames, but it will also keep track of a whole tree of coordinate frames,
+and automatically chain transformations together between any two connected frames.
+
+\section{Transform Library}
+
+\subsection{Terminology and Conventions}
+\paragraph {Coodinate Frame}
+\index{Coordinate Frame}
+In this documentation the default
+will be a right handed system with X forward, Y left and Z up.
+
+\paragraph {Denavit-Hartenberg Parameters (DH Parameters)}
+\index{Denavit-Hartenberg Parameters}
+DH Parameters are a way to concicely represent a rigid body tranformation in three dimentions.
+It has four parameters: length, twist, offset, and angle. In addition to using the optimal
+amount of data to store the transformation, there are also optimized methods of chaining
+transformations together. And lastly the parameters can directly represent rotary and prismatic
+joints found on robotic arms.
+See \url{http://en.wikipedia.org/wiki/Denavit-Hartenberg_Parameters} for more details.
+
+\todo{reproduce labeled diagram a good example \url{http://uwf.edu/ria/robotics/robotdraw/DH_parm.htm}}
+
+\paragraph {Euler Angles}
+\index{Euler Angles}
+For this library Euler angles are considered to be translations in x, y, z, followed by a rotation around z, y, x.
+With the respective angular changes referred to as yaw, pitch and roll.
+
+\paragraph {Homogeneous Transformation Matrix}
+\index{Homogeneous Transform}
+Homogeneous Transformation Matrices are a simple way to manipulate 3D translations and rotations
+with standard matrix multiplication. It is a composite of a standard 3x3 rotation matrix
+(see \url{http://en.wikipedia.org/wiki/Rotation_matrix}) and a translation vector.
+
+Let $_1R_0$ be the 3x3 rotation matrix defined by the Euler angles $(yaw_0,pitch_0,roll_0)$
+and let $_1T_0$ be the column vector $(x_0,y_0,z_0)^T$ representing the translation. The combination
+of these two transformations results in the transformation of reference frame 0 to reference frame 1.
+
+A point P in frame 1, $P_1$, can be transformed into frame 1, $P_0$, by the following:
+
+\begin{equation}
+\label{eqn:HT}
+\begin{pmatrix}
+P_0 \\
+1
+\end{pmatrix}
+=
+\begin{pmatrix}
+_0R_1 & _0T_1 \\
+1 & 1
+\end{pmatrix}
+\begin{pmatrix}
+P_1 \\
+1
+\end{pmatrix}
+\end{equation}
+
+The net result is a 4x4 transformation matrix which does both the rotation and translation
+between coordinate frames. The basic approach is to use a 4x1 vector consisting of $(x,y,z,1)^T_1$
+and by left multiplying by $_0A_1$ it will result in $(x',y',z',1)^T_1$.
+
+The matrix library used within this library is Newmat10.
+
+
+\paragraph {Newmat10}
+\index{Newmat10}
+Newmat10 is the matrix library used in this library. Documentation for Newmat can be found at
+\url{http://www.robertnz.net/nm10.htm}.
+
+\subsection{libTF API}
+\index{libTF API}
+The class which provides external functionality is named TransformReference.
+
+\subsubsection{Data Types}
+\begin{verbatim}
+typedef unsigned long long ULLtime;
+\end{verbatim}
+
+\texttt{ULLtime} provides a simple typedef for the timestamp used in this library.
+\texttt{ULLtime} is the number of nanoseconds since the epoch(1970) expressed as
+an unsigned long long, which should be equivilant to a \texttt{uint64\_t}.
+
+\subsubsection{Constructor}
+\index{libTF API!Constructor}
+\begin{verbatim}
+TransformReference(ULLtime cache_time = DEFAULT_CACHE_TIME);
+\end{verbatim}
+This is the constructor for the class. It's optional argument is
+how long to keep a history of the transforms. It is expressed in
+\texttt{ULLtime}, nanoseconds since the epoch(1970).
+
+\subsubsection{Mutators}
+
+\paragraph{setWithEulers}
+A method to set the parameters of a coordinate transform with Euler angles. \index{libTF API!setWithEulers}
+\begin{verbatim}
+void setWithEulers(unsigned int framid, unsigned int parentid,
+ double x, double y, double z,
+ double yaw, double pitch, double roll,
+ unsigned long long time);
+\end{verbatim}
+
+\paragraph{setWithDH}
+A method to set the parameters of a coordinate transform using DH Parameters. \index{libTF API!setWithDH}
+\begin{verbatim}
+void setWithDH(unsigned int framid, unsigned int parentid,
+ double length, double alpha,
+ double offset, double theta,
+ unsigned long long time);
+\end{verbatim}
+
+
+
+\paragraph{setWithMatrix}
+A method to set the parameters of a coordinate transform with a homogeneous transformation matrix. \index{libTF API!setWithMatrix}
+\begin{verbatim}
+unimplemented
+\end{verbatim}
+
+\paragraph{setWithQuaternion}
+A method to set the parameters of a coordinate transform with Quaternions. \index{libTF API!setWithQuaternion}
+\begin{verbatim}
+unimplemented
+\end{verbatim}
+
+
+\subsubsection{Accessors}
+\paragraph{getMatrix}
+\index{libTF API!getMatrix}
+\begin{verbatim}
+NEWMAT::Matrix getMatrix(unsigned int target_frame,
+ unsigned int source_frame,
+ unsigned long long time);
+\end{verbatim}
+
+\paragraph{viewChain}
+\index{libTF API!viewChain}
+\begin{verbatim}
+std::string viewChain(unsigned int target_frame,
+ unsigned int source_frame);
+\end{verbatim}
+
+\section{ROS Integration}
+
+unimplemented
+\subsection{libTF ROS API}
+unimplemented
+
+\section{Example Usage}
+\subsection{Library}
+%\begin{program}[H]
+\begin{verbatim}
+#include ``libTF/libTF.h''
+#include <time.h>
+
+using namespace std;
+
+int main(void)
+{
+ double dx,dy,dz,dyaw,dp,dr;
+ TransformReference mTR;
+
+ //Temporary Variables
+ dx = dy= dz = 0;
+ dyaw = dp = dr = 0.1;
+
+ unsigned long long atime = mTR.gettime();
+
+
+ //Fill in some transforms
+ // mTR.setWithEulers(10,2,1,1,1,dyaw,dp,dr,atime); //Switching out for DH par
+ams below
+ mTR.setWithDH(10,2,1.0,1.0,1.0,dyaw,atime);
+ //mTR.setWithEulers(2,3,1-1,1,1,dyaw,dp,dr,atime-1000);
+ mTR.setWithEulers(2,3,1,1,1,dyaw,dp,dr,atime-100);
+ mTR.setWithEulers(2,3,1,1,1,dyaw,dp,dr,atime-50);
+ mTR.setWithEulers(2,3,1,1,1,dyaw,dp,dr,atime-1000);
+ //mTR.setWithEulers(2,3,1+1,1,1,dyaw,dp,dr,atime+1000);
+ mTR.setWithEulers(3,5,dx,dy,dz,dyaw,dp,dr,atime);
+ mTR.setWithEulers(5,1,dx,dy,dz,dyaw,dp,dr,atime);
+ mTR.setWithEulers(6,5,dx,dy,dz,dyaw,dp,dr,atime);
+ mTR.setWithEulers(6,5,dx,dy,dz,dyaw,dp,dr,atime);
+ mTR.setWithEulers(7,6,1,1,1,dyaw,dp,dr,atime);
+ mTR.setWithDH(8,7,1.0,1.0,1.0,dyaw,atime);
+ //mTR.setWithEulers(8,7,1,1,1,dyaw,dp,dr,atime); //Switching out for DH params
+ above
+
+
+ //Demonstrate InvalidFrame LookupException
+ try
+ {
+ std::cout<< mTR.viewChain(10,9);
+ }
+ catch (TransformReference::LookupException &ex)
+ {
+ std::cout << ``Caught `` << ex.what()<<std::endl;
+ }
+
+
+ // See the list of transforms to get between the frames
+ std::cout<<''Viewing (10,8):''<<std::endl;
+ std::cout << mTR.viewChain(10,8);
+
+
+ //See the resultant transform
+ std::cout <<''Calling getMatrix(10,8)''<<std::endl;
+ NEWMAT::Matrix mat = mTR.getMatrix(10,8,atime);
+ std::cout << ``Result of getMatrix(10,8,atime):'' << std::endl << mat<< std::end
+l;
+
+ //Break the graph, making it loop and demonstrate catching MaxDepthException
+ mTR.setWithEulers(6,7,dx,dy,dz,dyaw,dp,dr,atime);
+
+ try {
+ std::cout<<mTR.viewChain(10,8);
+ }
+ catch (TransformReference::MaxDepthException &ex)
+ {
+ std::cout <<''caught loop in graph''<<std::endl;
+ }
+
+ //Break the graph, making it disconnected, and demonstrate catching Connectivi
+tyException
+ mTR.setWithEulers(6,0,dx,dy,dz,dyaw,dp,dr,atime);
+
+ try {
+ std::cout<<mTR.viewChain(10,8);
+ }
+ catch (TransformReference::ConnectivityException &ex)
+ {
+ std::cout <<''caught unconnected frame''<<std::endl;
+ }
+ return 0;
+};
+
+\end{verbatim}
+%\end{program}
+
+\subsection{ROS Implementation}
+\begin{verbatim}
+TODO
+\end{verbatim}
+
+\section{Summary of Internal Mathematics}
+This libarary uses quaternion notation as the internal representation
+of coordinate transforms. Transform information is stored in a linked
+list sorted by time. When a transform is requested the closest two points
+on the linked list are found and then interpolated in time to generate the
+return value.
+
+\subsection{Storage}
+The internal storage for the transforms consists of:
+\begin{struct}[H]
+\caption{libTF Internal Data Storage}
+\begin{verbatim}
+ double xtranslation;// The three components of translation
+ double ytranslation;
+ double ztranslation;
+ double xrotation; // The three components of the rotation axis
+ double yrotation;
+ double zrotation;
+ double w; // Omega
+ unsigned long long time; //nano seconds since 1970
+\end{verbatim}
+\end{struct}
+
+
+\subsection{Interpolation}
+The interpolation method used in this library is Spherical Linear Interpolation
+(SLERP). \index{SLERP} The standard formula for SLERP is defined in Equation \ref{eq:slerp}.
+The inputs are points $p_0$ and $p_1$, and $t$ is the proportion to interpolate
+between $p_0$ and $p_1$, and $\Omega$ is the angle between the axis of the two quaternions.
+
+\begin{equation}
+Slerp(p_0,p_1;t) = \frac{sin((1-t)\Omega)}{sin(\Omega)} * p_0 + \frac{sin(t*\Omega)}{sin(\Omega)} * p_1
+\label{eq:slerp}
+\end{equation}
+
+\todo{add graphic}
+
+\paragraph{Alternatives}
+The SLERP technique was developed in 1985 by Ken Shoemake. \cite{SHOEMAKE} SLERP has been
+largely adopted in the field of computer graphics, however there have been many alternatives
+developed. The advantages of SLERP are that it provides a constant speed solution
+along the shortest path over the 4D unit sphere. This is not optimal in all cases however.
+A good discussion of various alternatives is here \url{http://number-none.com/product/Understanding%20Slerp,%20Then%20Not%20Using%20It/index.html}.
+The approaches discussed above are normalized linear interpolation (nlerp), and
+log-quaternion linear interpolation (log-quaternion lerp).
+SLERP can be recursively called to do a cubic interpolation, called SQUAD for short.
+This is discussed at \url{http://www.sjbrown.co.uk/?article=quaternions}.
+
+
+
+\bibliographystyle{plain}
+\bibliography{libTF_Manual}
+
+\printindex
+\end{document}
+
Modified: pkg/branches/rosbus/libTF/include/libTF/Quaternion3D.h
===================================================================
--- pkg/branches/rosbus/libTF/include/libTF/Quaternion3D.h 2008-04-28 23:13:16 UTC (rev 216)
+++ pkg/branches/rosbus/libTF/include/libTF/Quaternion3D.h 2008-04-28 23:25:10 UTC (rev ...
[truncated message content] |
