Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[2363] pkg/trunk
|
From: <rob...@us...> - 2008-07-31 09:24:27
|
Revision: 2363
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=2363&view=rev
Author: rob_wheeler
Date: 2008-07-31 09:24:29 +0000 (Thu, 31 Jul 2008)
Log Message:
-----------
- Make a version of pr2_etherCAT that uses Stu's new mechanism_control.
- Deprecate robot_mechanism_model in favor of mechanism_model
- Deprecate genericControllers in favor of generic_controllers
- Bring various bits of code up to Brian's CppStyleGuide standards
Modified Paths:
--------------
pkg/trunk/drivers/motor/ethercat_hardware/include/ethercat_hardware/ethercat_hardware.h
pkg/trunk/drivers/motor/ethercat_hardware/include/ethercat_hardware/motor_control_board.h
pkg/trunk/drivers/motor/ethercat_hardware/include/ethercat_hardware/wg05.h
pkg/trunk/drivers/motor/ethercat_hardware/manifest.xml
pkg/trunk/drivers/motor/ethercat_hardware/src/ethercat_hardware.cpp
pkg/trunk/drivers/motor/ethercat_hardware/src/mk1001.cpp
pkg/trunk/drivers/motor/ethercat_hardware/src/wg05.cpp
pkg/trunk/robot_control_loops/pr2_etherCAT/manifest.xml
pkg/trunk/robot_control_loops/pr2_etherCAT/src/main.cpp
Added Paths:
-----------
pkg/trunk/deprecated/genericControllers/
pkg/trunk/deprecated/robot_mechanism_model/
pkg/trunk/mechanism/mechanism_control/
pkg/trunk/mechanism/mechanism_control/CMakeLists.txt
pkg/trunk/mechanism/mechanism_control/Makefile
pkg/trunk/mechanism/mechanism_control/include/
pkg/trunk/mechanism/mechanism_control/include/mechanism_control/
pkg/trunk/mechanism/mechanism_control/include/mechanism_control/mechanism_control.h
pkg/trunk/mechanism/mechanism_control/manifest.xml
pkg/trunk/mechanism/mechanism_control/src/
pkg/trunk/mechanism/mechanism_control/src/mechanism_control.cpp
pkg/trunk/mechanism/mechanism_model/
pkg/trunk/mechanism/mechanism_model/CMakeLists.txt
pkg/trunk/mechanism/mechanism_model/Makefile
pkg/trunk/mechanism/mechanism_model/include/
pkg/trunk/mechanism/mechanism_model/include/mechanism_model/
pkg/trunk/mechanism/mechanism_model/include/mechanism_model/joint.h
pkg/trunk/mechanism/mechanism_model/include/mechanism_model/link.h
pkg/trunk/mechanism/mechanism_model/include/mechanism_model/robot.h
pkg/trunk/mechanism/mechanism_model/include/mechanism_model/transmission.h
pkg/trunk/mechanism/mechanism_model/src/
pkg/trunk/mechanism/mechanism_model/src/simple_transmission.cpp
Removed Paths:
-------------
pkg/trunk/controllers/genericControllers/
pkg/trunk/robot_models/robot_mechanism_model/
Modified: pkg/trunk/drivers/motor/ethercat_hardware/include/ethercat_hardware/ethercat_hardware.h
===================================================================
--- pkg/trunk/drivers/motor/ethercat_hardware/include/ethercat_hardware/ethercat_hardware.h 2008-07-31 00:04:10 UTC (rev 2362)
+++ pkg/trunk/drivers/motor/ethercat_hardware/include/ethercat_hardware/ethercat_hardware.h 2008-07-31 09:24:29 UTC (rev 2363)
@@ -35,8 +35,10 @@
#ifndef ETHERCAT_HARDWARE_H
#define ETHERCAT_HARDWARE_H
-#include <hw_interface/hardware_interface.h>
+#include <tinyxml/tinyxml.h>
+#include <hardware_interface/hardware_interface.h>
+
#include <al/ethercat_AL.h>
#include <al/ethercat_master.h>
#include <al/ethercat_slave_handler.h>
@@ -64,15 +66,15 @@
/*!
* \brief Initialize the EtherCAT Master Library.
*/
- void init(char *interface, char *configuration);
+ void init(char *interface, TiXmlElement *config);
- HardwareInterface *hw;
+ HardwareInterface *hw_;
private:
- struct netif *ni;
+ struct netif *ni_;
- EtherCAT_AL *al;
- EtherCAT_Master *em;
+ EtherCAT_AL *al_;
+ EtherCAT_Master *em_;
MotorControlBoard *configSlave(EtherCAT_SlaveHandler *sh);
MotorControlBoard **slaves;
Modified: pkg/trunk/drivers/motor/ethercat_hardware/include/ethercat_hardware/motor_control_board.h
===================================================================
--- pkg/trunk/drivers/motor/ethercat_hardware/include/ethercat_hardware/motor_control_board.h 2008-07-31 00:04:10 UTC (rev 2362)
+++ pkg/trunk/drivers/motor/ethercat_hardware/include/ethercat_hardware/motor_control_board.h 2008-07-31 09:24:29 UTC (rev 2363)
@@ -40,7 +40,7 @@
#include <ethercat/ethercat_defs.h>
#include <al/ethercat_slave_handler.h>
-#include <hw_interface/hardware_interface.h>
+#include <hardware_interface/hardware_interface.h>
using namespace std;
Modified: pkg/trunk/drivers/motor/ethercat_hardware/include/ethercat_hardware/wg05.h
===================================================================
--- pkg/trunk/drivers/motor/ethercat_hardware/include/ethercat_hardware/wg05.h 2008-07-31 00:04:10 UTC (rev 2362)
+++ pkg/trunk/drivers/motor/ethercat_hardware/include/ethercat_hardware/wg05.h 2008-07-31 09:24:29 UTC (rev 2363)
@@ -39,35 +39,35 @@
struct WG05Status
{
- uint16_t deviceTypeId;
- uint16_t deviceRev;
- uint8_t mode;
- uint8_t digitalOut;
- uint16_t pwmDuty;
- uint16_t programmedCurrent;
- uint8_t currentLoopKp;
- uint8_t currentLoopKi;
- uint16_t measuredCurrent;
- uint16_t pad1;
- uint32_t timestamp;
- uint32_t encoderCount;
- uint32_t encoderIndexPos;
- uint16_t numEncoderErrors;
- uint8_t encoderStatus;
- uint8_t calibrationReading;
- uint32_t lastCalibrationHighTransition;
- uint32_t lastCalibrationLowTransition;
- uint16_t supplyVoltage;
- uint16_t motorVoltage;
- uint16_t boardTemperature;
- uint16_t bridgeTemperature;
- uint8_t pdoCommandIrqCount;
- uint8_t mbxCommandIrqCount;
- uint16_t packetCount;
- uint16_t pdiTimeoutErrorCount;
- uint16_t pdiChecksumErrorCount;
- uint8_t pad2;
- uint8_t checksum;
+ uint16_t device_type_id_;
+ uint16_t device_rev_;
+ uint8_t mode_;
+ uint8_t digital_out_;
+ uint16_t pwm_duty_;
+ uint16_t programmed_current_;
+ uint8_t current_loop_kp_;
+ uint8_t current_loop_ki_;
+ uint16_t measured_current_;
+ uint16_t pad1_;
+ uint32_t timestamp_;
+ uint32_t encoder_count_;
+ uint32_t encoder_index_pos_;
+ uint16_t num_encoder_errors_;
+ uint8_t encoder_status_;
+ uint8_t calibration_reading_;
+ uint32_t last_calibration_high_transition_;
+ uint32_t last_calibration_low_transition_;
+ uint16_t supply_voltage_;
+ uint16_t motor_voltage_;
+ uint16_t board_temperature_;
+ uint16_t bridge_temperature_;
+ uint8_t pdo_command_irq_count_;
+ uint8_t mbx_command_irq_count_;
+ uint16_t packet_count_;
+ uint16_t pdi_timeout_error_count_;
+ uint16_t pdi_checksum_error_count_;
+ uint8_t pad2_;
+ uint8_t checksum_;
}__attribute__ ((__packed__));
typedef WG05Status WG05Command;
@@ -92,7 +92,7 @@
MotorControlBoard(WG05_PRODUCT_CODE, sizeof(WG05Command), sizeof(WG05Status))
{
}
- void configure(int &startAddress, EtherCAT_SlaveHandler *sh);
+ void configure(int &start_address, EtherCAT_SlaveHandler *sh);
void convertCommand(ActuatorCommand &command, unsigned char *buffer);
void convertState(ActuatorState &state, unsigned char *current_buffer, unsigned char *last_buffer);
bool hasActuator(void)
Modified: pkg/trunk/drivers/motor/ethercat_hardware/manifest.xml
===================================================================
--- pkg/trunk/drivers/motor/ethercat_hardware/manifest.xml 2008-07-31 00:04:10 UTC (rev 2362)
+++ pkg/trunk/drivers/motor/ethercat_hardware/manifest.xml 2008-07-31 09:24:29 UTC (rev 2363)
@@ -5,8 +5,9 @@
<author>Rob Wheeler (email: wh...@wi...)</author>
<license>BSD</license>
<url>http://pr.willowgarage.com</url>
-<depend package='hw_interface'/>
+<depend package='hardware_interface'/>
<depend package='eml'/>
+<depend package='tinyxml'/>
<export>
<cpp cflags="-I${prefix}/include" lflags="-L${prefix}/lib -lethercat_hardware -Wl,-rpath,${prefix}/lib"/>
</export>
Modified: pkg/trunk/drivers/motor/ethercat_hardware/src/ethercat_hardware.cpp
===================================================================
--- pkg/trunk/drivers/motor/ethercat_hardware/src/ethercat_hardware.cpp 2008-07-31 00:04:10 UTC (rev 2362)
+++ pkg/trunk/drivers/motor/ethercat_hardware/src/ethercat_hardware.cpp 2008-07-31 09:24:29 UTC (rev 2363)
@@ -39,15 +39,15 @@
#include <dll/ethercat_dll.h>
EthercatHardware::EthercatHardware() :
- hw(0), ni(0), current_buffer_(0), last_buffer_(0), buffer_size_(0)
+ hw_(0), ni_(0), current_buffer_(0), last_buffer_(0), buffer_size_(0)
{
}
EthercatHardware::~EthercatHardware()
{
- if (ni)
+ if (ni_)
{
- close_socket(ni);
+ close_socket(ni_);
}
if (current_buffer_)
{
@@ -57,50 +57,50 @@
{
delete last_buffer_;
}
- if (hw)
+ if (hw_)
{
- delete hw;
+ delete hw_;
}
}
-void EthercatHardware::init(char *interface, char *configuration)
+void EthercatHardware::init(char *interface, TiXmlElement *configuration)
{
// Initialize network interface
- if ((ni = init_ec(interface)) == NULL)
+ if ((ni_ = init_ec(interface)) == NULL)
{
perror("init_ec");
return;
}
// Initialize Application Layer (AL)
- EtherCAT_DataLinkLayer::instance()->attach(ni);
- if ((al = EtherCAT_AL::instance()) == NULL)
+ EtherCAT_DataLinkLayer::instance()->attach(ni_);
+ if ((al_ = EtherCAT_AL::instance()) == NULL)
{
perror("EtherCAT_AL::instance");
return;
}
- unsigned int numSlaves = al->get_num_slaves();
- if (numSlaves == 0)
+ unsigned int num_slaves = al_->get_num_slaves();
+ if (num_slaves == 0)
{
perror("Can't locate any slaves");
return;
}
// Initialize Master
- if ((em = EtherCAT_Master::instance()) == NULL)
+ if ((em_ = EtherCAT_Master::instance()) == NULL)
{
perror("EtherCAT_Master::instance");
return;
}
- slaves = new MotorControlBoard*[numSlaves];
+ slaves = new MotorControlBoard*[num_slaves];
unsigned int num_actuators = 0;
- for (unsigned int slave = 0; slave < numSlaves; ++slave)
+ for (unsigned int slave = 0; slave < num_slaves; ++slave)
{
EC_FixedStationAddress fsa(slave + 1);
- EtherCAT_SlaveHandler *sh = em->get_slave_handler(fsa);
+ EtherCAT_SlaveHandler *sh = em_->get_slave_handler(fsa);
if (sh == NULL)
{
perror("get_slave_handler");
@@ -123,7 +123,7 @@
// Determine configuration from XML file 'configuration'
// Create HardwareInterface
- hw = new HardwareInterface(num_actuators);
+ hw_ = new HardwareInterface(num_actuators);
}
void EthercatHardware::update()
@@ -132,21 +132,21 @@
// Convert HW Interface commands to MCB-specific buffers
current = current_buffer_;
- for (int i = 0; i < hw->numActuators; ++i)
+ for (int i = 0; i < hw_->num_actuators_; ++i)
{
- slaves[i]->convertCommand(hw->actuator[i].command, current);
+ slaves[i]->convertCommand(hw_->actuators_[i].command_, current);
current += slaves[i]->commandSize + slaves[i]->statusSize;
}
// Transmit process data
- em->txandrx_PD(buffer_size_, current_buffer_);
+ em_->txandrx_PD(buffer_size_, current_buffer_);
// Convert status back to HW Interface
current = current_buffer_;
last = last_buffer_;
- for (int i = 0; i < hw->numActuators; ++i)
+ for (int i = 0; i < hw_->num_actuators_; ++i)
{
- slaves[i]->convertState(hw->actuator[i].state, current, last);
+ slaves[i]->convertState(hw_->actuators_[i].state_, current, last);
current += slaves[i]->commandSize + slaves[i]->statusSize;
last += slaves[i]->commandSize + slaves[i]->statusSize;
}
Modified: pkg/trunk/drivers/motor/ethercat_hardware/src/mk1001.cpp
===================================================================
--- pkg/trunk/drivers/motor/ethercat_hardware/src/mk1001.cpp 2008-07-31 00:04:10 UTC (rev 2362)
+++ pkg/trunk/drivers/motor/ethercat_hardware/src/mk1001.cpp 2008-07-31 09:24:29 UTC (rev 2363)
@@ -101,8 +101,8 @@
c.config = 1;
c.i_offset = 0;//-2000;
- c.i_desire = command.current;
- c.mode = command.enable ? MODE_PWM : MODE_OFF;
+ c.i_desire = command.current_;
+ c.mode = command.enable_ ? MODE_PWM : MODE_OFF;
memcpy(buffer + sizeof(MK1001Status), &c, sizeof(c));
}
@@ -114,10 +114,10 @@
memcpy(&s, buffer, sizeof(s));
memcpy(&c, buffer + sizeof(s), sizeof(c));
- state.isEnabled = c.mode != MODE_OFF;
- state.timestamp = s.timestamp;
- state.encoderCount = s.qei_pos;
- state.encoderVelocity = s.qei_velocity;
- state.lastMeasuredCurrent = s.adc_current;
+ state.is_enabled_ = c.mode != MODE_OFF;
+ state.timestamp_ = s.timestamp;
+ state.encoder_count_ = s.qei_pos;
+ state.encoder_velocity_ = s.qei_velocity;
+ state.last_measured_current_ = s.adc_current;
}
Modified: pkg/trunk/drivers/motor/ethercat_hardware/src/wg05.cpp
===================================================================
--- pkg/trunk/drivers/motor/ethercat_hardware/src/wg05.cpp 2008-07-31 00:04:10 UTC (rev 2362)
+++ pkg/trunk/drivers/motor/ethercat_hardware/src/wg05.cpp 2008-07-31 09:24:29 UTC (rev 2363)
@@ -93,10 +93,10 @@
memset(&c, 0, sizeof(c));
- c.currentLoopKi = Ki;
+ c.current_loop_ki_ = Ki;
- c.programmedCurrent = CURRENT_FACTOR * command.current;
- c.mode = command.enable ? (MODE_ENABLE | MODE_CURRENT) : MODE_OFF;
+ c.programmed_current_ = CURRENT_FACTOR * command.current_;
+ c.mode_ = command.enable_ ? (MODE_ENABLE | MODE_CURRENT) : MODE_OFF;
memcpy(buffer + sizeof(WG05Status), &c, sizeof(c));
}
@@ -112,44 +112,23 @@
memcpy(&last_status, last_buffer, sizeof(last_status));
memcpy(&last_command, last_buffer + sizeof(last_status), sizeof(last_command));
- state.encoderCount = current_status.encoderCount;
- state.encoderVelocity = double(int(current_status.encoderCount - last_status.encoderCount)) / (current_status.timestamp - last_status.timestamp) * 1e+6;
- state.calibrationReading = current_status.calibrationReading;
- state.lastCalibrationHighTransition = current_status.lastCalibrationHighTransition;
- state.lastCalibrationLowTransition = current_status.lastCalibrationLowTransition;
- state.isEnabled = current_status.mode != MODE_OFF;
- state.runStopHit = (current_status.mode & MODE_UNDERVOLTAGE) != 0;
+ state.encoder_count_ = current_status.encoder_count_;
+ state.encoder_velocity_ = double(int(current_status.encoder_count_ - last_status.encoder_count_)) / (current_status.timestamp_ - last_status.timestamp_) * 1e+6;
+ state.calibration_reading_ = current_status.calibration_reading_;
+ state.last_calibration_high_transition_ = current_status.last_calibration_high_transition_;
+ state.last_calibration_low_transition_ = current_status.last_calibration_low_transition_;
+ state.is_enabled_ = current_status.mode_ != MODE_OFF;
+ state.run_stop_hit_ = (current_status.mode_ & MODE_UNDERVOLTAGE) != 0;
- state.lastRequestedCurrent = current_command.programmedCurrent / CURRENT_FACTOR; // Should be actual request, before safety
- state.lastCommandedCurrent = current_status.programmedCurrent / CURRENT_FACTOR;
- state.lastMeasuredCurrent = current_status.measuredCurrent / CURRENT_FACTOR;
+ state.last_requested_current_ = current_command.programmed_current_ / CURRENT_FACTOR; // TODO should be pre-safety code value
+ state.last_commanded_current_ = current_status.programmed_current_ / CURRENT_FACTOR;
+ state.last_measured_current_ = current_status.measured_current_ / CURRENT_FACTOR;
- state.numEncoderErrors = current_status.numEncoderErrors;
- state.numCommunicationErrors = current_status.pdiTimeoutErrorCount + current_status.pdiChecksumErrorCount;
+ state.num_encoder_errors_ = current_status.num_encoder_errors_;
+ state.num_communication_errors_ = current_status.pdi_timeout_error_count_ + current_status.pdi_checksum_error_count_;
- state.motorVoltage = current_status.motorVoltage;
-#if 1
-{
-static int times = 0;
+ state.motor_voltage_ = current_status.motor_voltage_;
-if (++times % 2000 == 0) {
- printf("-----------------------------------------------------\n");
- printf("timestamp = %#08x\n", current_status.timestamp);
- printf("encoderCount = %#08x\n", current_status.encoderCount);
- printf("last timestamp = %#08x\n", last_status.timestamp);
- printf("last encoderCount = %#08x\n", last_status.encoderCount);
- printf("delta encoderCount = %#08x %f\n", current_status.encoderCount - last_status.encoderCount, double(int(current_status.encoderCount - last_status.encoderCount)));
- printf("delta timestamp = %#08x %f\n", current_status.timestamp - last_status.timestamp, double(current_status.timestamp - last_status.timestamp));
- printf("encoderVelocity = %f\n", state.encoderVelocity);
- printf("programmedCurrent = %#08x\n", current_status.programmedCurrent);
- printf("measuredCurrent = %#08x\n", current_status.measuredCurrent);
- printf("mode = %#08x\n", current_status.mode);
- printf("Kp = %#08x\n", int(current_status.currentLoopKp));
- printf("Ki = %#08x\n", int(current_status.currentLoopKi));
}
-}
-#endif
-}
-
Added: pkg/trunk/mechanism/mechanism_control/CMakeLists.txt
===================================================================
--- pkg/trunk/mechanism/mechanism_control/CMakeLists.txt (rev 0)
+++ pkg/trunk/mechanism/mechanism_control/CMakeLists.txt 2008-07-31 09:24:29 UTC (rev 2363)
@@ -0,0 +1,5 @@
+cmake_minimum_required(VERSION 2.6)
+include(rosbuild)
+rospack(mechanism_control)
+rospack_add_library(mechanism_control src/mechanism_control.cpp)
+
Added: pkg/trunk/mechanism/mechanism_control/Makefile
===================================================================
--- pkg/trunk/mechanism/mechanism_control/Makefile (rev 0)
+++ pkg/trunk/mechanism/mechanism_control/Makefile 2008-07-31 09:24:29 UTC (rev 2363)
@@ -0,0 +1 @@
+include $(shell rospack find mk)/cmake.mk
Added: pkg/trunk/mechanism/mechanism_control/include/mechanism_control/mechanism_control.h
===================================================================
--- pkg/trunk/mechanism/mechanism_control/include/mechanism_control/mechanism_control.h (rev 0)
+++ pkg/trunk/mechanism/mechanism_control/include/mechanism_control/mechanism_control.h 2008-07-31 09:24:29 UTC (rev 2363)
@@ -0,0 +1,65 @@
+
+///////////////////////////////////////////////////////////////////////////////
+// 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_CONTROL_H
+#define MECHANISM_CONTROL_H
+
+#include <map>
+#include <string>
+#include <vector>
+#include <tinyxml/tinyxml.h>
+#include <hardware_interface/hardware_interface.h>
+#include <mechanism_model/robot.h>
+#include <rosthread/mutex.h>
+#include <generic_controllers/controller.h>
+
+typedef controller::Controller* (*ControllerAllocator)();
+
+class MechanismControl {
+public:
+ MechanismControl(HardwareInterface *hw);
+ ~MechanismControl();
+
+ bool init(TiXmlElement* config);
+ void update(); //Must be realtime safe
+
+ bool registerActuator(const std::string &name, int index);
+ void registerControllerType(const std::string& type, ControllerAllocator f);
+ bool spawnController(const char* type, TiXmlElement* config);
+
+private:
+ bool initialized_;
+ mechanism::Robot model_;
+ HardwareInterface *hw_;
+
+ std::map<std::string, ControllerAllocator> controller_library_;
+ const static int MAX_NUM_CONTROLLERS = 100;
+ ros::thread::mutex controllers_mutex_;
+ controller::Controller* controllers_[MAX_NUM_CONTROLLERS];
+};
+
+#endif /* MECHANISM_CONTROL_H */
Added: pkg/trunk/mechanism/mechanism_control/manifest.xml
===================================================================
--- pkg/trunk/mechanism/mechanism_control/manifest.xml (rev 0)
+++ pkg/trunk/mechanism/mechanism_control/manifest.xml 2008-07-31 09:24:29 UTC (rev 2363)
@@ -0,0 +1,16 @@
+<package>
+<description brief="Mechanism control">
+</description>
+<author>Eric Berger be...@wi...</author>
+<license>BSD</license>
+<url>http://pr.willowgarage.com</url>
+<depend package="rosthread"/>
+<depend package="hardware_interface"/>
+<depend package="tinyxml"/>
+<depend package="mechanism_model"/>
+<depend package="generic_controllers"/>
+<export>
+ <cpp cflags='-I${prefix}/include' lflags='-Wl,-rpath,${prefix}/lib -L${prefix}/lib -lmechanism_control'/>
+</export>
+</package>
+
Added: pkg/trunk/mechanism/mechanism_control/src/mechanism_control.cpp
===================================================================
--- pkg/trunk/mechanism/mechanism_control/src/mechanism_control.cpp (rev 0)
+++ pkg/trunk/mechanism/mechanism_control/src/mechanism_control.cpp 2008-07-31 09:24:29 UTC (rev 2363)
@@ -0,0 +1,172 @@
+
+///////////////////////////////////////////////////////////////////////////////
+// Copyright (C) 2008, Willow Garage, Inc.
+//
+// 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 Willow Garage, Inc. 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 "mechanism_control/mechanism_control.h"
+
+using namespace mechanism;
+
+MechanismControl::MechanismControl(HardwareInterface *hw)
+ : initialized_(0), model_((char*)"robot"), hw_(hw)
+{
+ memset(controllers_, 0, MAX_NUM_CONTROLLERS*sizeof(void*));
+ model_.hw_ = hw;
+}
+
+MechanismControl::~MechanismControl() {
+}
+
+bool MechanismControl::registerActuator(const std::string &name, int index)
+{
+ if (initialized_)
+ return false;
+
+ model_.actuators_lookup_.insert(Robot::IndexMap::value_type(name, index));
+
+ return true;
+}
+
+bool MechanismControl::init(TiXmlElement* config)
+{
+ bool successful = true;
+
+ TiXmlElement *elt;
+
+ // Constructs the joints
+ std::map<std::string, Joint*> joint_map;
+ for (elt = config->FirstChildElement("joint"); elt; elt = elt->NextSiblingElement("joint"))
+ {
+ Joint *j = new Joint;
+ model_.joints_.push_back(j);
+ model_.joints_lookup_.insert(
+ Robot::IndexMap::value_type(elt->Attribute("name"), model_.joints_.size() - 1));
+ j->joint_limit_min_ = atof(elt->FirstChildElement("limitMin")->GetText());
+ j->joint_limit_max_ = atof(elt->FirstChildElement("limitMax")->GetText());
+ j->effort_limit_ = atof(elt->FirstChildElement("effortLimit")->GetText());
+ j->velocity_limit_ = atof(elt->FirstChildElement("velocityLimit")->GetText());
+ }
+
+ // Constructs the transmissions
+ elt = config->FirstChildElement("transmission");
+ for (; elt; elt = elt->NextSiblingElement("transmission"))
+ {
+ if (0 == strcmp("SimpleTransmission", elt->Attribute("type")))
+ {
+ // Looks up the joint and the actuator used by the transmission.
+ Robot::IndexMap::iterator joint_it =
+ model_.joints_lookup_.find(elt->FirstChildElement("joint")->Attribute("name"));
+ Robot::IndexMap::iterator actuator_it =
+ model_.actuators_lookup_.find(elt->FirstChildElement("actuator")->Attribute("name"));
+ if (joint_it == model_.joints_lookup_.end())
+ {
+ // TODO: report: The joint was not declared in the XML file
+ continue;
+ }
+ if (actuator_it == model_.actuators_lookup_.end())
+ {
+ // TODO: report: The actuator was not registered with mechanism control.
+ continue;
+ }
+
+ Transmission *tr = new SimpleTransmission
+ (model_.joints_[joint_it->second], &hw_->actuators_[actuator_it->second],
+ atof(elt->FirstChildElement("mechanicalReduction")->Value()),
+ atof(elt->FirstChildElement("motorTorqueConstant")->Value()),
+ atof(elt->FirstChildElement("pulsesPerRevolution")->Value()));
+ model_.transmissions_.push_back(tr);
+ }
+ else
+ {
+ // TODO: report: Unknown transmission type
+ successful = false;
+ }
+ }
+
+ initialized_ = true;
+ return successful;
+}
+
+// Must be realtime safe.
+void MechanismControl::update() {
+
+ // Propagates through the robot model.
+ for (unsigned int i = 0; i < model_.transmissions_.size(); ++i)
+ model_.transmissions_[i]->propagatePosition();
+
+ // TODO: update KDL model with new joint position/velocities
+
+ //update all controllers
+ for(int i = 0; i < MAX_NUM_CONTROLLERS; ++i){
+ if(controllers_[i] != NULL)
+ controllers_[i]->update();
+ }
+
+ // Performs safety checks on the commands.
+ for (unsigned int i = 0; i < model_.joints_.size(); ++i)
+ model_.joints_[i]->enforceLimits();
+
+ // Propagates commands back into the actuators.
+ for (unsigned int i = 0; i < model_.transmissions_.size(); ++i)
+ model_.transmissions_[i]->propagateEffort();
+}
+
+
+
+void MechanismControl::registerControllerType(const std::string& type, ControllerAllocator f){
+ controller_library_.insert(std::pair<std::string,ControllerAllocator>(type, f));
+}
+
+bool MechanismControl::spawnController(const char *type, TiXmlElement *config)
+{
+ controller::Controller *c;
+ ControllerAllocator f = controller_library_[type];
+ c = f();
+ c->initXml(&model_, config);
+
+ //At this point, the controller is fully initialized and has created the ROS interface, etc.
+
+ //Add controller to list of controllers in realtime-safe manner;
+ controllers_mutex_.lock(); //This lock is only to prevent us from other non-realtime threads. The realtime thread may be spinning through the list of controllers while we are in here, so we need to keep that list always in a valid state. This is why we fully allocate and set up the controller before adding it into the list of active controllers.
+ bool spot_found = false;
+ for(int i = 0; i < MAX_NUM_CONTROLLERS; i++){
+ if(controllers_[i] == NULL)
+ {
+ spot_found = true;
+ controllers_[i] = c;
+ break;
+ }
+ }
+ controllers_mutex_.unlock();
+
+ if (!spot_found)
+ {
+ delete c;
+ return false;
+ }
+
+ return true;
+}
Added: pkg/trunk/mechanism/mechanism_model/CMakeLists.txt
===================================================================
--- pkg/trunk/mechanism/mechanism_model/CMakeLists.txt (rev 0)
+++ pkg/trunk/mechanism/mechanism_model/CMakeLists.txt 2008-07-31 09:24:29 UTC (rev 2363)
@@ -0,0 +1,4 @@
+cmake_minimum_required(VERSION 2.6)
+include(rosbuild)
+rospack(mechanism_model)
+rospack_add_library(mechanism_model src/simple_transmission.cpp)
Added: pkg/trunk/mechanism/mechanism_model/Makefile
===================================================================
--- pkg/trunk/mechanism/mechanism_model/Makefile (rev 0)
+++ pkg/trunk/mechanism/mechanism_model/Makefile 2008-07-31 09:24:29 UTC (rev 2363)
@@ -0,0 +1 @@
+include $(shell rospack find mk)/cmake.mk
Added: pkg/trunk/mechanism/mechanism_model/include/mechanism_model/joint.h
===================================================================
--- pkg/trunk/mechanism/mechanism_model/include/mechanism_model/joint.h (rev 0)
+++ pkg/trunk/mechanism/mechanism_model/include/mechanism_model/joint.h 2008-07-31 09:24:29 UTC (rev 2363)
@@ -0,0 +1,77 @@
+/*********************************************************************
+ * 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 JOINT_H
+#define JOINT_H
+
+namespace mechanism {
+
+class Joint{
+public:
+ void enforceLimits()
+ {
+ // TODO: enforce the limits so the joint operates safely
+ }
+
+ char *name_;
+ int type_;
+
+ //Update every cycle from input data
+ bool initialized_;
+ double position_;
+ double velocity_;
+ double applied_effort_;
+
+ //Written every cycle out to motor boards
+ double commanded_effort_;
+
+ //Never changes
+ double joint_limit_min_;
+ double joint_limit_max_;
+ double effort_limit_;
+ double velocity_limit_;
+};
+
+enum
+{
+ JOINT_NONE,
+ JOINT_ROTARY,
+ JOINT_CONTINUOUS,
+ JOINT_PRISMATIC,
+ JOINT_FIXED,
+ JOINT_TYPES_MAX
+};
+
+}
+
+#endif /* JOINT_H */
Added: pkg/trunk/mechanism/mechanism_model/include/mechanism_model/link.h
===================================================================
--- pkg/trunk/mechanism/mechanism_model/include/mechanism_model/link.h (rev 0)
+++ pkg/trunk/mechanism/mechanism_model/include/mechanism_model/link.h 2008-07-31 09:24:29 UTC (rev 2363)
@@ -0,0 +1,50 @@
+/*********************************************************************
+ * 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 LINK_H
+#define LINK_H
+
+#include "mechanism_model/joint.h"
+
+namespace mechanism {
+
+class Link
+{
+ char *name;
+ //Pointer to KDL link
+ Joint *joint;
+};
+
+}
+
+#endif /* LINK_H */
Added: pkg/trunk/mechanism/mechanism_model/include/mechanism_model/robot.h
===================================================================
--- pkg/trunk/mechanism/mechanism_model/include/mechanism_model/robot.h (rev 0)
+++ pkg/trunk/mechanism/mechanism_model/include/mechanism_model/robot.h 2008-07-31 09:24:29 UTC (rev 2363)
@@ -0,0 +1,93 @@
+/*********************************************************************
+ * 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.
+ *********************************************************************/
+//The robot model is populated by the control code infrastructure and used by all the controllers to read mechanism state and command mechanism motion.
+
+#ifndef ROBOT_H
+#define ROBOT_H
+
+#include <vector>
+#include <map>
+#include <string>
+#include "mechanism_model/link.h"
+#include "mechanism_model/joint.h"
+#include "mechanism_model/transmission.h"
+#include "hardware_interface/hardware_interface.h"
+
+namespace mechanism
+{
+
+class Robot
+{
+public:
+ Robot(char *ns){}
+
+ ~Robot()
+ {
+ std::vector<Transmission *>::size_type t;
+ for (t = 0; t < transmissions_.size(); ++t)
+ delete transmissions_[t];
+ std::vector<Joint *>::size_type j;
+ for (j = 0; j < joints_.size(); ++j)
+ delete joints_[j];
+ }
+
+ std::vector<Joint*> joints_;
+ std::vector<Transmission*> transmissions_;
+
+ // Supports looking up joints and actuators by name. The IndexMap
+ // structure maps the name of the item to its index in the vectors.
+ typedef std::map<std::string,int> IndexMap;
+ IndexMap joints_lookup_;
+ IndexMap actuators_lookup_;
+ Joint* getJoint(const std::string &name)
+ {
+ IndexMap::iterator it = joints_lookup_.find(name);
+ if (it == joints_lookup_.end())
+ return NULL;
+ return joints_[it->second];
+ }
+ Actuator* getActuator(const std::string &name)
+ {
+ IndexMap::iterator it = actuators_lookup_.find(name);
+ if (it == actuators_lookup_.end())
+ return NULL;
+ return &hw_->actuators_[it->second];
+ }
+
+ HardwareInterface *hw_;
+};
+
+}
+
+#endif
Added: pkg/trunk/mechanism/mechanism_model/include/mechanism_model/transmission.h
===================================================================
--- pkg/trunk/mechanism/mechanism_model/include/mechanism_model/transmission.h (rev 0)
+++ pkg/trunk/mechanism/mechanism_model/include/mechanism_model/transmission.h 2008-07-31 09:24:29 UTC (rev 2363)
@@ -0,0 +1,76 @@
+/*********************************************************************
+ * 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 TRANSMISSION_H
+#define TRANSMISSION_H
+
+#include "mechanism_model/joint.h"
+#include "hardware_interface/hardware_interface.h"
+
+namespace mechanism {
+
+class Transmission
+{
+public:
+ Transmission() {}
+ virtual ~Transmission() {}
+
+ // Uses encoder data to fill out joint position and velocities
+ virtual void propagatePosition() = 0;
+
+ // Uses commanded joint efforts to fill out commanded motor currents
+ virtual void propagateEffort() = 0;
+};
+
+
+class SimpleTransmission : public Transmission
+{
+public:
+ SimpleTransmission() {}
+ SimpleTransmission(Joint *joint, Actuator *actuator, double mechanical_reduction, double motor_torque_constant, double ticks_per_radian);
+ ~SimpleTransmission() {}
+
+ Actuator *actuator_;
+ Joint *joint_;
+
+ double mechanical_reduction_;
+ double motor_torque_constant_;
+ double pulses_per_revolution_;
+
+ void propagatePosition();
+ void propagateEffort();
+};
+
+} // namespace mechanism
+
+#endif
Added: pkg/trunk/mechanism/mechanism_model/src/simple_transmission.cpp
===================================================================
--- pkg/trunk/mechanism/mechanism_model/src/simple_transmission.cpp (rev 0)
+++ pkg/trunk/mechanism/mechanism_model/src/simple_transmission.cpp 2008-07-31 09:24:29 UTC (rev 2363)
@@ -0,0 +1,63 @@
+/*********************************************************************
+ * 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 <mechanism_model/transmission.h>
+#include <mechanism_model/joint.h>
+#include <math.h>
+#include <stdio.h>
+
+using namespace mechanism;
+
+SimpleTransmission::SimpleTransmission(Joint *joint, Actuator *actuator,
+ double mechanical_reduction, double motor_torque_constant,
+ double pulses_per_revolution)
+{
+ actuator_ = actuator;
+ mechanical_reduction_ = mechanical_reduction;
+ motor_torque_constant_ = motor_torque_constant;
+ pulses_per_revolution_ = pulses_per_revolution;
+ joint_ = joint;
+}
+
+
+void SimpleTransmission::propagatePosition()
+{
+ joint_->position_ = ((double)actuator_->state_.encoder_count_*2*M_PI)/(pulses_per_revolution_ * mechanical_reduction_);
+ joint_->velocity_ = ((double)actuator_->state_.encoder_velocity_*2*M_PI)/(pulses_per_revolution_ * mechanical_reduction_);
+ joint_->applied_effort_ = actuator_->state_.last_measured_current_ * (motor_torque_constant_ * mechanical_reduction_);
+}
+
+void SimpleTransmission::propagateEffort()
+{
+ actuator_->command_.current_ = joint_->commanded_effort_/(motor_torque_constant_ * mechanical_reduction_);
+}
Modified: pkg/trunk/robot_control_loops/pr2_etherCAT/manifest.xml
===================================================================
--- pkg/trunk/robot_control_loops/pr2_etherCAT/manifest.xml 2008-07-31 00:04:10 UTC (rev 2362)
+++ pkg/trunk/robot_control_loops/pr2_etherCAT/manifest.xml 2008-07-31 09:24:29 UTC (rev 2363)
@@ -5,9 +5,9 @@
<author>Rob Wheeler/wh...@wi...</author>
<license>BSD</license>
<depend package="roscpp" />
- <depend package="old_mechanism_control"/>
+ <depend package="mechanism_control"/>
<depend package="ethercat_hardware"/>
- <depend package="hw_interface"/>
+ <depend package="hardware_interface"/>
<url>http://pr.willowgarage.com</url>
<repository>http://pr.willowgarage.com/repos</repository>
Modified: pkg/trunk/robot_control_loops/pr2_etherCAT/src/main.cpp
===================================================================
--- pkg/trunk/robot_control_loops/pr2_etherCAT/src/main.cpp 2008-07-31 00:04:10 UTC (rev 2362)
+++ pkg/trunk/robot_control_loops/pr2_etherCAT/src/main.cpp 2008-07-31 09:24:29 UTC (rev 2363)
@@ -32,7 +32,9 @@
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
-#include <mechanism_control/single_control.h>
+#include <ros/node.h>
+
+#include <mechanism_control/mechanism_control.h>
#include <ethercat_hardware/ethercat_hardware.h>
#include <stdio.h>
@@ -52,14 +54,21 @@
void *controlLoop(void *arg)
{
- char *interface = (char *)arg;
+ char **args = (char **) arg;
+ char *interface = args[1];
+ char *xml_file = args[2];
+ TiXmlDocument xml(xml_file);
+ xml.LoadFile();
+ TiXmlElement *root = xml.FirstChildElement("robot");
+printf("root = %08x\n", root);
+
EthercatHardware ec;
- ec.init(interface, (char *)"foo.xml");
- getTime(ec.hw);
+ ec.init(interface, root);
+ getTime(ec.hw_);
- SingleControl mc;
- mc.init(ec.hw);
+ MechanismControl mc(ec.hw_);
+ mc.init(root);
// Switch to hard real-time
int period = 1e+6; // 1 ms in nanoseconds
@@ -72,7 +81,7 @@
while (!quit)
{
ec.update();
- getTime(ec.hw);
+ getTime(ec.hw_);
mc.update();
tick.tv_nsec += period;
while (tick.tv_nsec >= NSEC_PER_SEC)
@@ -83,7 +92,7 @@
clock_nanosleep(CLOCK_REALTIME, TIMER_ABSTIME, &tick, NULL);
}
- memset(ec.hw->actuator, 0, ec.hw->numActuators * sizeof(Actuator));
+ memset(ec.hw_->actuators_, 0, ec.hw_->num_actuators_ * sizeof(Actuator));
ec.update();
return 0;
@@ -112,9 +121,9 @@
{
// Initialize ROS and check command-line arguments
ros::init(argc, argv);
- if (argc != 2)
+ if (argc != 3)
{
- fprintf(stderr, "Usage: %s <interface>\n", argv[0]);
+ fprintf(stderr, "Usage: %s <interface> <xml>\n", argv[0]);
exit(-1);
}
@@ -138,7 +147,7 @@
//Start thread
int rv;
- if ((rv = pthread_create(&rtThread, &rtThreadAttr, controlLoop, argv[1])) != 0)
+ if ((rv = pthread_create(&rtThread, &rtThreadAttr, controlLoop, argv)) != 0)
{
perror("pthread_create");
exit(1);
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
