Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[2845] pkg/trunk
|
From: <rob...@us...> - 2008-08-08 23:49:56
|
Revision: 2845
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=2845&view=rev
Author: rob_wheeler
Date: 2008-08-08 23:50:01 +0000 (Fri, 08 Aug 2008)
Log Message:
-----------
pr2_etherCat:
- extract configuration from xml file
- get current time in ethercat_hardware
mechanism_model:
- Initialize joints from xml element
mechanism_control:
- Move Joint initialization into Joint's initXml
- Fix transmission xml parsing
- When initializing actuators, push_back onto vector instead of assigning
generic_controllers:
- Replace all singing/all dancing JointController with simplified JointPositionController
ethercat_hardware:
- Register actuators with mechanism control
- Fix some sign problems in wg05 command packet
- Set HardwareInterface's current time in update loop
Modified Paths:
--------------
pkg/trunk/controllers/generic_controllers/CMakeLists.txt
pkg/trunk/controllers/generic_controllers/include/generic_controllers/controller.h
pkg/trunk/drivers/motor/ethercat_hardware/include/ethercat_hardware/ethercat_hardware.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/wg05.cpp
pkg/trunk/mechanism/hardware_interface/include/hardware_interface/hardware_interface.h
pkg/trunk/mechanism/mechanism_control/src/mechanism_control.cpp
pkg/trunk/mechanism/mechanism_model/CMakeLists.txt
pkg/trunk/mechanism/mechanism_model/include/mechanism_model/joint.h
pkg/trunk/mechanism/mechanism_model/manifest.xml
pkg/trunk/mechanism/mechanism_model/src/simple_transmission.cpp
pkg/trunk/robot_control_loops/pr2_etherCAT/src/main.cpp
Added Paths:
-----------
pkg/trunk/controllers/generic_controllers/include/generic_controllers/joint_position_controller.h
pkg/trunk/controllers/generic_controllers/src/joint_position_controller.cpp
pkg/trunk/mechanism/mechanism_model/src/joint.cpp
Removed Paths:
-------------
pkg/trunk/controllers/generic_controllers/include/generic_controllers/joint_controller.h
pkg/trunk/controllers/generic_controllers/src/joint_controller.cpp
Modified: pkg/trunk/controllers/generic_controllers/CMakeLists.txt
===================================================================
--- pkg/trunk/controllers/generic_controllers/CMakeLists.txt 2008-08-08 23:44:45 UTC (rev 2844)
+++ pkg/trunk/controllers/generic_controllers/CMakeLists.txt 2008-08-08 23:50:01 UTC (rev 2845)
@@ -1,4 +1,4 @@
cmake_minimum_required(VERSION 2.6)
include(rosbuild)
rospack(generic_controllers)
-rospack_add_library(generic_controllers src/pid.cpp src/joint_controller.cpp)
+rospack_add_library(generic_controllers src/pid.cpp src/joint_position_controller.cpp)
Modified: pkg/trunk/controllers/generic_controllers/include/generic_controllers/controller.h
===================================================================
--- pkg/trunk/controllers/generic_controllers/include/generic_controllers/controller.h 2008-08-08 23:44:45 UTC (rev 2844)
+++ pkg/trunk/controllers/generic_controllers/include/generic_controllers/controller.h 2008-08-08 23:50:01 UTC (rev 2845)
@@ -45,7 +45,8 @@
#include <misc_utils/factory.h>
#include <mechanism_model/robot.h>
-class TiXmlElement;
+#include <tinyxml/tinyxml.h>
+//class TiXmlElement;
namespace controller
{
@@ -82,7 +83,6 @@
{
}
virtual void update(void) = 0;
- virtual void init(void) = 0;
virtual void initXml(mechanism::Robot *robot, TiXmlElement *config) = 0;
};
Deleted: pkg/trunk/controllers/generic_controllers/include/generic_controllers/joint_controller.h
===================================================================
--- pkg/trunk/controllers/generic_controllers/include/generic_controllers/joint_controller.h 2008-08-08 23:44:45 UTC (rev 2844)
+++ pkg/trunk/controllers/generic_controllers/include/generic_controllers/joint_controller.h 2008-08-08 23:50:01 UTC (rev 2845)
@@ -1,343 +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.
- *********************************************************************/
-#pragma once
-/***************************************************/
-/*! \class controller::JointController
- \brief A Joint controller
-
- This class implements controller loops for
- PR2 Joint Control
-
- ASSUMES:
- Rotary joint
-
- Parameters to be set by SetParam
- PGain
- IGain
- DGain
- IMax
- IMin
-
- Parameters fetched from joint
- Time
- SaturationEffort
- MaxEffort
-
- Steps to bring a JointController online
- 1. Initialize gains via SetParam
- 2. Set the controller mode (SetMode(mode))
- 3. EnableController()
- 4. Set appropriate command (position, torque, velocity)
- 5. Call Update from Real Time loop
-
-
- */
-/***************************************************/
-#include <sys/types.h>
-#include <stdint.h>
-
-#include <generic_controllers/controller.h>
-#include <generic_controllers/pid.h>
-#include <math_utils/angles.h>
-#include <mechanism_model/joint.h>
-#include <string>
-#include <math.h>
-#include <urdf/URDF.h>
-
-namespace controller
-{
-
-class JointController : public Controller
-{
- static const double ACCELERATION_THRESHOLD = 0.1; //Distance threshold below which linear acceleration limit is active, if enabled
-public:
- /*!
- * \brief Default Constructor of the JointController class.
- *
- */
- JointController();
-
- /*!
- * \brief Destructor of the JointController class.
- */
- ~JointController();
-
- static Controller* create()
- {
- return new JointController();
- }
-
- void init(void);
- void initXml(mechanism::Robot *robot, TiXmlElement *config);
-
- /*!
- * \brief Functional way to initialize limits and gains.
- *
- */
- void init(PidControlParam pcp, ControllerControlMode mode, double time, double maxEffort, double minEffort,
- mechanism::Joint *joint);
-
- /*!
- * \brief Functional way to initialize limits and gains.
- *
- */
- void init(double time, mechanism::Joint *joint);
-
- /*!
- * \brief Functional way to initialize limits and gains.
- *
- */
- void init(double pGain, double iGain, double dGain, double windupMax, double windupMin, ControllerControlMode mode,
- double time, double maxEffort, double minEffort, mechanism::Joint *joint);
-
- //-------------------------------------------------------------------------//
- //MODE/ENABLE CALLS
- //-------------------------------------------------------------------------//
-
- /*!
- * \brief Switches command mode type (Torque, position, velocity control)
- *
- */
- ControllerControlMode setMode(ControllerControlMode mode);
-
- /*!
- * \brief Returns the current mode of the controller
- */
- ControllerControlMode getMode(void);
-
- /*!
- * \brief Allow controller to run
- */
- ControllerControlMode enableController();
-
- /*!
- * \brief Set torque to zero. Prevent controller from running
- */
- ControllerControlMode disableController();
-
- /*!
- * \brief Return true if last command saturated the torque
- *
- *
- */
- bool checkForSaturation(void);
-
- //-------------------------------------------------------------------------//
- //TORQUE CALLS
- //-------------------------------------------------------------------------//
- /*!
- * \brief Give a torque command to be issue on update (if in torque mode)
- *
- * \param torque Torque command to issue
- */
- ControllerErrorCode setTorqueCmd(double torque);
-
- /*!
- * \brief Fetch the latest user issued torque command
- *
- * \param double* torque Pointer to value to change
- */
- ControllerErrorCode getTorqueCmd(double *torque);
-
- /*!
- * \brief Get the actual torque of the joint motor.
- *
- * \param double* torque Pointer to value to change
- */
- ControllerErrorCode getTorqueAct(double *torque);
-
- //-------------------------------------------------------------------------//
- //POSITION CALLS
- //-------------------------------------------------------------------------//
-
- /*!
- * \brief Give set position of the joint for next update: revolute (angle) and prismatic (position)
- *
- * \param double pos Position command to issue
- */
- ControllerErrorCode setPosCmd(double pos);
-
- /*!
- * \brief Get latest position command to the joint: revolute (angle) and prismatic (position).
- * \param double* pos Pointer to value to change
- */
- ControllerErrorCode getPosCmd(double *pos);
-
- /*!
- * \brief Read the torque of the motor
- * \param double* pos Pointer to value to change
- */
- ControllerErrorCode getPosAct(double *pos);
-
- //-------------------------------------------------------------------------//
- //VELOCITY CALLS
- //-------------------------------------------------------------------------//
-
- /*!
- * \brief Set velocity command to the joint to be issue next update
- * \param double vel Velocity to issue next command
- */
- ControllerErrorCode setVelCmd(double vel);
-
- /*!
- * \brief Get latest velocity command to the joint
- * \param double* vel Pointer to value to change
- */
- ControllerErrorCode getVelCmd(double *vel);
-
- /*!
- * \brief Get actual velocity of the joint
- * \param double* vel Pointer to value to change
- */
- ControllerErrorCode getVelAct(double *vel);
-
- /*!
- * \brief Compute max velocity coming into the end stop to stop with linear velocity in endstop.
- *
- */
- double getMaxVelocity(void);
-
- //-------------------------------------------------------------------------//
- //UPDATE CALLS
- //-------------------------------------------------------------------------//
- /*!
- * \brief Issues commands to joint based on control mode
- *
- *
- */
-
- //Issues commands to the joint. Should be called at regular intervals
- virtual void update(void);
-
- //-------------------------------------------------------------------------//
- //PARAM SERVER CALLS
- //-------------------------------------------------------------------------//
- /*!
- * \brief Set parameters for this controller
- *
- * user can set maximum velocity
- * and maximum acceleration
- * constraints for this controller
- * \param string label Name of param to change
- * \param double value New value
- *<br>
- *<UL TYPE="none">
- *<LI> e.g. SetParam('PGain',10);
- *<LI> or SetParam('IGain',10);
- *<LI> or SetParam('DGain',1);
- *<LI> or SetParam('IMax', 100);
- *<LI> or SetParam('IMin',-100);
- *</UL>
- */
- ControllerErrorCode setParam(std::string label, double value);
-
- /*!
- * \brief Get parameters for this controller
- *
- * user can get maximum velocity
- * and maximum acceleration
- * constraints for this controller
- *<br>
- *<UL TYPE="none">
- *<UL TYPE="none">
- *<LI> e.g. GetParam('PGain',value);
- *<LI> or GetParam('IGain',value);
- *<LI> or GetParam('DGain',value);
- *<LI> or GetParam('IMax', value);
- *<LI> or GetParam('IMin', value);
- *</UL>
- */
- ControllerErrorCode getParam(std::string label, double* value);
-
- /*!
- * \brief Set the name of the controller
- *
- * \param name - std::string representation of the name of the controller
- */
- void setName(const std::string & name);
-
- /*!
- * \brief Return string name of the controller
- *
- * \return std::string representation of the name of the controller
- */
- std::string getName();
-
- bool cap_accel_; /*!<Flag to indicate whether we should cap acceleration.>*/
- double max_accel_; /*!<Maximum allowed acceleration/deceleration.>*/
-
- /*!
- * \brief load parameters from the XML file
- *
- * \param std::string representation of the filename for the XML file with complete path
- */
- ControllerErrorCode loadXML(std::string filename);
-
-private:
-
- /*!
- * \brief Actually issue torque set command of the joint motor.
- */
- void setJointEffort(double torque);
-
- mechanism::Joint* joint_; /*!< Joint we're controlling>*/
- Pid pid_controller_; /*!< Internal PID controller>*/
-
- double last_time_;/*!< Last time stamp of update> */
-
- ControllerControlMode control_mode_; /*!< Indicate current controller mode (torque, position, velocity)>*/
- double cmd_torque_;/*!< Last commanded torque>*/
- double cmd_pos_;/*!< Last commanded position> */
- double cmd_vel_;/*!< Last commanded Velocity> */
-
- bool saturation_flag_; /*!< Flag to indicate last command exceed torque limits and was truncated>*/
- bool enabled_; /*!<Can controller issue commands?>*/
-
- double p_gain_; /*!< Proportional gain>*/
- double i_gain_;/*!< Integral gain >*/
- double d_gain_;/*!< Derivative gain> */
- double windup_max_;/*!< Upper integral clamp> */
- double windup_min_;/*!< Lower integral clamp> */
- double max_effort_; /*!<Temporary (until param server) : local copy of max effort.>*/
- double min_effort_; /*!<Temporary (until param server): local copy of min effort.>*/
-
- std::map<std::string,std::string> param_map_;
- void loadParam(std::string label, double &value);
- void loadParam(std::string label, int &value);
- std::string name_; /*!< Namespace ID for this controller>*/
-
- mechanism::Robot *robot_;
-};
-
-}
Added: pkg/trunk/controllers/generic_controllers/include/generic_controllers/joint_position_controller.h
===================================================================
--- pkg/trunk/controllers/generic_controllers/include/generic_controllers/joint_position_controller.h (rev 0)
+++ pkg/trunk/controllers/generic_controllers/include/generic_controllers/joint_position_controller.h 2008-08-08 23:50:01 UTC (rev 2845)
@@ -0,0 +1,102 @@
+/*********************************************************************
+ * 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_POSITION_CONTROLLER_H
+#define JOINT_POSITION_CONTROLLER_H
+
+#include <generic_controllers/controller.h>
+#include <generic_controllers/pid.h>
+
+namespace controller
+{
+
+class JointPositionController : public Controller
+{
+public:
+ /*!
+ * \brief Default Constructor of the JointController class.
+ *
+ */
+ JointPositionController();
+
+ /*!
+ * \brief Destructor of the JointController class.
+ */
+ ~JointPositionController();
+
+ /*!
+ * \brief Functional way to initialize limits and gains.
+ *
+ */
+ void init(double p_gain, double i_gain, double d_gain, double windup, double time, mechanism::Joint *joint);
+ void initXml(mechanism::Robot *robot, TiXmlElement *config);
+
+ /*!
+ * \brief Give set position of the joint for next update: revolute (angle) and prismatic (position)
+ *
+ * \param double pos Position command to issue
+ */
+ void setCommand(double command);
+
+ /*!
+ * \brief Get latest position command to the joint: revolute (angle) and prismatic (position).
+ */
+ double getCommand();
+
+ /*!
+ * \brief Read the torque of the motor
+ */
+ double getActual();
+
+ /*!
+ * \brief Issues commands to the joint. Should be called at regular intervals
+ */
+
+ virtual void update();
+
+private:
+ /*!
+ * \brief Actually issue torque set command of the joint motor.
+ */
+ void setJointEffort(double torque);
+
+ mechanism::Joint* joint_; /*!< Joint we're controlling>*/
+ Pid pid_controller_; /*!< Internal PID controller>*/
+ double last_time_; /*!< Last time stamp of update> */
+ double command_; /*!< Last commanded position> */
+ mechanism::Robot *robot_; /*!< Pointer to robot structure>*/
+};
+}
+
+#endif /* JOINT_POSITION_CONTROLLER_H */
Deleted: pkg/trunk/controllers/generic_controllers/src/joint_controller.cpp
===================================================================
--- pkg/trunk/controllers/generic_controllers/src/joint_controller.cpp 2008-08-08 23:44:45 UTC (rev 2844)
+++ pkg/trunk/controllers/generic_controllers/src/joint_controller.cpp 2008-08-08 23:50:01 UTC (rev 2845)
@@ -1,479 +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 <generic_controllers/joint_controller.h>
-#define DEFAULTMAXACCEL 5
-//#define DEBUG 1
-//Todo:
-//1. Get and set params via server
-//2. Integrate Joint and robot objects
-//3. Integrate Motor controller time
-
-using namespace controller;
-
-ROS_REGISTER_CONTROLLER(JointController)
-
-//-------------------------------------------------------------------------//
-//CONSTRUCTION/DESTRUCTION CALLS
-//-------------------------------------------------------------------------//
-
-
-JointController::JointController()
-{
- //Instantiate PID class
- pid_controller_.initPid(0, 0, 0, 0, 0); //Constructor for pid controller
-
- //Set commands to zero
- cmd_torque_ = 0;
- cmd_pos_ = 0;
- cmd_vel_ = 0;
-
- control_mode_ = CONTROLLER_DISABLED;
-}
-
-JointController::~JointController()
-{
-}
-
-void JointController::init()
-{
-}
-
-void JointController::initXml(mechanism::Robot *robot, TiXmlElement *config)
-{
-}
-
-void JointController::init(double p_gain, double i_gain, double d_gain, double windup_max, double windup_min,
- ControllerControlMode mode, double time, double max_effort, double min_effort,
- mechanism::Joint *joint)
-{
- pid_controller_.initPid(p_gain, i_gain, d_gain, windup_max, windup_min); //Constructor for pid controller
-
- cmd_torque_ = 0;
- cmd_pos_ = 0;
- cmd_vel_ = 0;
-
- //Init time
- last_time_ = time;
-
- //Temporary: will transition to use param server
- p_gain_ = p_gain;
- i_gain_ = i_gain;
- d_gain_ = d_gain;
- windup_max_ = windup_max;
- windup_min_ = windup_min;
-
- min_effort_ = min_effort;
- max_effort_ = max_effort;
- joint_ = joint;
-
- control_mode_ = mode;
- enableController();
-}
-
-void JointController::init(PidControlParam pcp, ControllerControlMode mode, double time, double max_effort,
- double min_effort, mechanism::Joint *joint)
-{
- pid_controller_.initPid(pcp.p_gain_, pcp.i_gain_, pcp.d_gain_, pcp.windup_max_, pcp.windup_min_); //Constructor for pid controller
-
- cmd_torque_ = 0;
- cmd_pos_ = 0;
- cmd_vel_ = 0;
-
- //Init time
- last_time_ = time;
-
- //Temporary: will transition to use param server
- p_gain_ = pcp.p_gain_;
- i_gain_ = pcp.i_gain_;
- d_gain_ = pcp.d_gain_;
- windup_max_ = pcp.windup_max_;
- windup_min_ = pcp.windup_min_;
-
- min_effort_ = min_effort;
- max_effort_ = max_effort;
- joint_ = joint;
-
- control_mode_ = mode;
- enableController();
-}
-
-void JointController::init(double time, mechanism::Joint *joint)
-{
- pid_controller_.initPid(p_gain_, i_gain_, d_gain_, windup_max_, windup_min_); //Constructor for pid controller
-
- cmd_torque_ = 0;
- cmd_pos_ = 0;
- cmd_vel_ = 0;
-
- last_time_ = time;
- joint_ = joint;
- enableController();
-}
-
-//-------------------------------------------------------------------------//
-//MODE/ENABLE CALLS
-//-------------------------------------------------------------------------//
-
-//Set the controller control mode
-ControllerControlMode JointController::setMode(ControllerControlMode mode)
-{
- control_mode_ = mode;
- return CONTROLLER_MODE_SET;
-}
-
-//Getter for control mode
-ControllerControlMode JointController::getMode()
-{
- return control_mode_;
-}
-
-//Allow controller to function
-ControllerControlMode JointController::enableController()
-{
- enabled_ = true;
- return CONTROLLER_ENABLED;
-}
-
-//Disable functioning. Set joint torque to zero.
-ControllerControlMode JointController::disableController()
-{
- enabled_ = false;
- joint_->commanded_effort_ = 0; //Immediately set commanded Effort to 0
- control_mode_ = CONTROLLER_DISABLED;
- return CONTROLLER_DISABLED;
-}
-
-bool JointController::checkForSaturation(void)
-{
- return saturation_flag_;
-}
-
-//-------------------------------------------------------------------------//
-//TORQUE CALLS
-//-------------------------------------------------------------------------//
-ControllerErrorCode JointController::setTorqueCmd(double torque)
-{
- // double max_effort = joint->effortLimit;
-
- if (control_mode_ != CONTROLLER_TORQUE) //Make sure we're in torque command mode
- {
- return CONTROLLER_MODE_ERROR;
- }
-
- cmd_torque_ = torque;
-
- if (cmd_torque_ >= max_effort_)
- { //Truncate to positive limit
- cmd_torque_ = max_effort_;
- return CONTROLLER_TORQUE_LIMIT;
- }
- else if (cmd_torque_ <= min_effort_)
- { //Truncate to negative limit
- cmd_torque_ = min_effort_;
- return CONTROLLER_TORQUE_LIMIT;
- }
-
- return CONTROLLER_CMD_SET;
-}
-
-ControllerErrorCode JointController::getTorqueCmd(double *torque)
-{
- *torque = cmd_torque_;
- return CONTROLLER_ALL_OK;
-}
-
-ControllerErrorCode JointController::getTorqueAct(double *torque)
-{
- *torque = joint_->applied_effort_; //Read torque from joint
- return CONTROLLER_ALL_OK;
-}
-
-//-------------------------------------------------------------------------//
-//POSITION CALLS
-//-------------------------------------------------------------------------//
-
-
-//Query mode, then set desired position
-ControllerErrorCode JointController::setPosCmd(double pos)
-{
- if (control_mode_ != CONTROLLER_POSITION) //Make sure we're in position command mode
- {
- return CONTROLLER_MODE_ERROR;
- }
-
- cmd_pos_ = pos;
- if (cmd_pos_ >= joint_->joint_limit_max_ && joint_->type_ != mechanism::JOINT_CONTINUOUS)
- { //Truncate to positive limit
- cmd_pos_ = joint_->joint_limit_max_;
- return CONTROLLER_JOINT_LIMIT;
- }
- else if (cmd_pos_ <= joint_->joint_limit_min_ && joint_->type_ != mechanism::JOINT_CONTINUOUS)
- { //Truncate to negative limit
- cmd_pos_ = joint_->joint_limit_min_;
- return CONTROLLER_JOINT_LIMIT;
- }
- return CONTROLLER_CMD_SET;
-}
-
-//Return the current position command
-controller::ControllerErrorCode JointController::getPosCmd(double *pos)
-{
- *pos = cmd_pos_;
- return controller::CONTROLLER_ALL_OK;
-}
-
-//Query the joint for the actual position
-controller::ControllerErrorCode JointController::getPosAct(double *pos)
-{
- *pos = joint_->position_;
- return controller::CONTROLLER_ALL_OK;
-}
-
-//-------------------------------------------------------------------------//
-//VELOCITY CALLS
-//-------------------------------------------------------------------------//
-//Check mode, then set the commanded velocity
-ControllerErrorCode JointController::setVelCmd(double vel)
-{
- if (control_mode_ == CONTROLLER_VELOCITY || control_mode_ == ETHERDRIVE_SPEED)
- { //Make sure we're in velocity command mode
- cmd_vel_ = vel;
- return CONTROLLER_CMD_SET;
- }
- else
- return CONTROLLER_MODE_ERROR;
-}
-
-//Return the internally stored commanded velocity
-controller::ControllerErrorCode JointController::getVelCmd(double *vel)
-{
- *vel = cmd_vel_;
- return controller::CONTROLLER_ALL_OK;
-}
-
-//Query our joint for velocity
-controller::ControllerErrorCode JointController::getVelAct(double *vel)
-{
- *vel = joint_->velocity_;
- return controller::CONTROLLER_ALL_OK;
-}
-
-double JointController::getMaxVelocity()
-{
- double dis_to_min, dis_to_max, closest_limit;
- dis_to_min = fabs(math_utils::shortest_angular_distance(joint_->position_, joint_->joint_limit_min_));
- dis_to_max = fabs(math_utils::shortest_angular_distance(joint_->position_, joint_->joint_limit_max_));
- closest_limit = dis_to_min < dis_to_max ? dis_to_min : dis_to_max;
- return sqrt(fabs(closest_limit * max_accel_));
-}
-
-//-------------------------------------------------------------------------//
-//UPDATE CALLS
-//-------------------------------------------------------------------------//
-
-
-void JointController::update(void)
-{
- double error(0), time(0), current_torque_cmd(0);
- double max_velocity = cmd_vel_;
- double currentVoltageCmd, v_backemf, v_clamp_min, v_clamp_max, k;
-
- if (control_mode_ == controller::CONTROLLER_DISABLED)
- {
- printf("JointController.cpp: Error:: controller disabled\n");
- return; //If we're not initialized, don't try to interact
- }
- time = robot_->hw_->current_time_;
- switch (control_mode_)
- {
- case CONTROLLER_TORQUE: //Pass through torque command
- current_torque_cmd = cmd_torque_;
- break;
-
- case CONTROLLER_POSITION: //Close the loop around position
- if (joint_->type_ == mechanism::JOINT_ROTARY || joint_->type_ == mechanism::JOINT_CONTINUOUS)
- error = math_utils::shortest_angular_distance(cmd_pos_, joint_->position_);
- else
- error = joint_->position_ - cmd_pos_;
- current_torque_cmd = pid_controller_.updatePid(error, time - last_time_);
- break;
-
- case CONTROLLER_VELOCITY: //Close the loop around velocity
- if (cap_accel_)
- {
- max_velocity = getMaxVelocity(); //Check max velocity coming into wall
- if (fabs(cmd_vel_) > max_velocity)
- {
- cmd_vel_ = -max_velocity; //Truncate velocity smoothly
- }
- }
- error = joint_->velocity_ - cmd_vel_;
-
- current_torque_cmd = pid_controller_.updatePid(error, time - last_time_);
- break;
- case ETHERDRIVE_SPEED: // Use hack to contol speed in voltage control mode for the etherdrive
-#ifdef DEBUG
- printf("JC:: %f\n",cmd_vel_);
-#endif
- currentVoltageCmd = cmd_vel_ * 20 * 60 / (136 * 2 * M_PI);
- v_backemf = joint_->velocity_ * 20 * 60 / (136 * 2 * M_PI);
-
- v_clamp_min = v_backemf - 3;// 0.655*16.7;
- v_clamp_max = v_backemf + 3;//0.655*16.7;
-
- k = 1.0 / 36.0;
-#ifdef DEBUG
- printf("JC::%f\t%f\t%f\n", v_clamp_min, currentVoltageCmd, v_clamp_max);
-#endif
- if (currentVoltageCmd > v_clamp_max)
- currentVoltageCmd = v_clamp_max;
-
- if (currentVoltageCmd < v_clamp_min)
- currentVoltageCmd = v_clamp_min;
- current_torque_cmd = currentVoltageCmd * k; //Convert to match PWM conversion inside boards
- break;
-
- default:
- printf("JointController.cpp: Error:: invalid control_mode_\n");
- current_torque_cmd = 0;
- }
- last_time_ = time;
-
- setJointEffort(current_torque_cmd);
-}
-
-ControllerErrorCode JointController::setParam(std::string label, double value)
-{
- return CONTROLLER_ALL_OK;
-}
-
-ControllerErrorCode JointController::getParam(std::string label, double* value)
-{
- return CONTROLLER_ALL_OK;
-}
-
-//Truncates (if needed), then sets the effort
-void JointController::setJointEffort(double effort)
-{
- double new_effort;
-
- new_effort = effort;
- saturation_flag_ = false;
-
- if (effort >= max_effort_)
- {
- new_effort = max_effort_;
- saturation_flag_ = true;
- }
- else if (effort <= min_effort_)
- {
- new_effort = min_effort_;
- saturation_flag_ = true;
- }
- joint_->commanded_effort_ = new_effort;
-}
-
-ControllerErrorCode JointController::loadXML(std::string filename)
-{
- robot_desc::URDF model;
- int exists = 0;
-
- if (!model.loadFile(filename.c_str()))
- return CONTROLLER_MODE_ERROR;
-
- const robot_desc::URDF::Data &data = model.getData();
-
- std::vector<std::string> types;
- std::vector<std::string> names;
- std::vector<std::string>::iterator iter;
-
- data.getDataTagTypes(types);
-
- for (iter = types.begin(); iter != types.end(); iter++)
- {
- if (*iter == "controller")
- {
- exists = 1;
- break;
- }
- }
-
- if (!exists)
- return CONTROLLER_MODE_ERROR;
-
- exists = 0;
- data.getDataTagNames("controller", names);
-
- for (iter = names.begin(); iter != names.end(); iter++)
- {
- if (*iter == name_)
- {
- exists = 1;
- break;
- }
- }
-
- if (!exists)
- return CONTROLLER_MODE_ERROR;
-
- param_map_ = data.getDataTagValues("controller", name_);
-
- loadParam("p_gain", p_gain_);
- loadParam("d_gain", d_gain_);
- loadParam("i_gain", i_gain_);
- loadParam("windup_max", windup_max_);
- loadParam("windup_min", windup_min_);
- loadParam("max_effort", max_effort_);
- loadParam("min_effort", min_effort_);
-
- return CONTROLLER_ALL_OK;
-}
-
-void JointController::loadParam(std::string label, double ¶m)
-{
- if (param_map_.find(label) != param_map_.end()) // if parameter value has been initialized in the xml file, set internal parameter value
- param = atof(param_map_[label].c_str());
-}
-
-void JointController::loadParam(std::string label, int ¶m)
-{
- if (param_map_.find(label) != param_map_.end())
- param = atoi(param_map_[label].c_str());
-}
-
-std::string JointController::getName()
-{
- return name_;
-}
-
Added: pkg/trunk/controllers/generic_controllers/src/joint_position_controller.cpp
===================================================================
--- pkg/trunk/controllers/generic_controllers/src/joint_position_controller.cpp (rev 0)
+++ pkg/trunk/controllers/generic_controllers/src/joint_position_controller.cpp 2008-08-08 23:50:01 UTC (rev 2845)
@@ -0,0 +1,123 @@
+/*********************************************************************
+ * 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 <algorithm>
+
+#include <generic_controllers/joint_position_controller.h>
+#include <math_utils/angles.h>
+
+using namespace std;
+using namespace controller;
+
+ROS_REGISTER_CONTROLLER(JointPositionController)
+
+JointPositionController::JointPositionController()
+{
+ // Initialize PID class
+ pid_controller_.initPid(0, 0, 0, 0, 0);
+ command_ = 0;
+}
+
+JointPositionController::~JointPositionController()
+{
+}
+
+void JointPositionController::init(double p_gain, double i_gain, double d_gain, double windup, double time, mechanism::Joint *joint)
+{
+ pid_controller_.initPid(p_gain, i_gain, d_gain, windup, -windup);
+
+ command_= 0;
+ last_time_= time;
+ joint_ = joint;
+}
+
+void JointPositionController::initXml(mechanism::Robot *robot, TiXmlElement *config)
+{
+ robot_ = robot;
+ TiXmlElement *elt = config->FirstChildElement("joint");
+ if (elt) {
+ // TODO: error check if xml attributes/elements are missing
+ double p_gain = atof(elt->FirstChildElement("pGain")->GetText());
+ double i_gain = atof(elt->FirstChildElement("iGain")->GetText());
+ double d_gain = atof(elt->FirstChildElement("dGain")->GetText());
+ double windup= atof(elt->FirstChildElement("windup")->GetText());
+ init(p_gain, i_gain, d_gain, windup, robot->hw_->current_time_, robot->getJoint(elt->Attribute("name")));
+ }
+}
+
+// Set the joint position command
+void JointPositionController::setCommand(double command)
+{
+ command_ = command;
+}
+
+// Return the current position command
+double JointPositionController::getCommand()
+{
+ return command_;
+}
+
+// Return the measured joint position
+double JointPositionController::getActual()
+{
+ return joint_->position_;
+}
+
+void JointPositionController::update()
+{
+ double error(0);
+ double torque_cmd(0);
+ double time = robot_->hw_->current_time_;
+
+ if(joint_->type_ == mechanism::JOINT_ROTARY || joint_->type_ == mechanism::JOINT_CONTINUOUS)
+ {
+ error = math_utils::shortest_angular_distance(command_, joint_->position_);
+ }
+ else
+ {
+ error = joint_->position_ - command_;
+ }
+
+ torque_cmd = pid_controller_.updatePid(error, time - last_time_);
+
+ setJointEffort(torque_cmd);
+}
+
+void JointPositionController::setJointEffort(double effort)
+{
+ joint_->commanded_effort_ = min(max(effort, -joint_->effort_limit_), joint_->effort_limit_);
+}
+
+
+
+
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-08-08 23:44:45 UTC (rev 2844)
+++ pkg/trunk/drivers/motor/ethercat_hardware/include/ethercat_hardware/ethercat_hardware.h 2008-08-08 23:50:01 UTC (rev 2845)
@@ -37,6 +37,7 @@
#include <tinyxml/tinyxml.h>
+#include <mechanism_control/mechanism_control.h>
#include <hardware_interface/hardware_interface.h>
#include <al/ethercat_AL.h>
@@ -66,8 +67,13 @@
/*!
* \brief Initialize the EtherCAT Master Library.
*/
- void init(char *interface, TiXmlElement *config);
+ void init(char *interface);
+ /*!
+ * \brief Register actuators with mechanism control
+ */
+ void initXml(TiXmlElement *config, MechanismControl &mc);
+
HardwareInterface *hw_;
private:
Modified: pkg/trunk/drivers/motor/ethercat_hardware/include/ethercat_hardware/wg05.h
===================================================================
--- pkg/trunk/drivers/motor/ethercat_hardware/include/ethercat_hardware/wg05.h 2008-08-08 23:44:45 UTC (rev 2844)
+++ pkg/trunk/drivers/motor/ethercat_hardware/include/ethercat_hardware/wg05.h 2008-08-08 23:50:01 UTC (rev 2845)
@@ -44,10 +44,10 @@
uint8_t mode_;
uint8_t digital_out_;
uint16_t pwm_duty_;
- uint16_t programmed_current_;
+ int16_t programmed_current_;
uint8_t current_loop_kp_;
uint8_t current_loop_ki_;
- uint16_t measured_current_;
+ int16_t measured_current_;
uint16_t pad1_;
uint32_t timestamp_;
uint32_t encoder_count_;
Modified: pkg/trunk/drivers/motor/ethercat_hardware/manifest.xml
===================================================================
--- pkg/trunk/drivers/motor/ethercat_hardware/manifest.xml 2008-08-08 23:44:45 UTC (rev 2844)
+++ pkg/trunk/drivers/motor/ethercat_hardware/manifest.xml 2008-08-08 23:50:01 UTC (rev 2845)
@@ -9,6 +9,7 @@
<depend package='eml'/>
<depend package='tinyxml'/>
<depend package='roscpp' />
+<depend package='mechanism_control' />
<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-08-08 23:44:45 UTC (rev 2844)
+++ pkg/trunk/drivers/motor/ethercat_hardware/src/ethercat_hardware.cpp 2008-08-08 23:50:01 UTC (rev 2845)
@@ -65,8 +65,17 @@
}
}
-void EthercatHardware::init(char *interface, TiXmlElement *configuration)
+static const int NSEC_PER_SEC = 1e+9;
+
+static double now()
{
+ struct timespec now;
+ clock_gettime(CLOCK_REALTIME, &now);
+ return double(now.tv_nsec) / NSEC_PER_SEC + now.tv_sec;
+}
+
+void EthercatHardware::init(char *interface)
+{
ros::node *node = ros::node::instance();
// Initialize network interface
@@ -120,12 +129,23 @@
current_buffer_ = buffers_;
last_buffer_ = buffers_ + buffer_size_;
- // Determine configuration from XML file 'configuration'
// Create HardwareInterface
hw_ = new HardwareInterface(num_actuators);
+ hw_->current_time_ = now();
}
+void EthercatHardware::initXml(TiXmlElement *config, MechanismControl &mc)
+{
+ int i = 0;
+ // Determine configuration from XML file 'configuration'
+ // TODO: match actuator name to name supplied by board
+ for (TiXmlElement *elt = config->FirstChildElement("actuator"); elt; elt = elt->NextSiblingElement("actuator"))
+ {
+ mc.registerActuator(elt->Attribute("name"), i++);
+ }
+}
+
void EthercatHardware::update()
{
unsigned char *current, *last;
@@ -151,6 +171,9 @@
last += slaves[i]->commandSize + slaves[i]->statusSize;
}
+ // Update current time
+ hw_->current_time_ = now();
+
unsigned char *tmp = current_buffer_;
current_buffer_ = last_buffer_;
last_buffer_ = tmp;
Modified: pkg/trunk/drivers/motor/ethercat_hardware/src/wg05.cpp
===================================================================
--- pkg/trunk/drivers/motor/ethercat_hardware/src/wg05.cpp 2008-08-08 23:44:45 UTC (rev 2844)
+++ pkg/trunk/drivers/motor/ethercat_hardware/src/wg05.cpp 2008-08-08 23:50:01 UTC (rev 2845)
@@ -128,7 +128,4 @@
state.num_communication_errors_ = current_status.pdi_timeout_error_count_ + current_status.pdi_checksum_error_count_;
state.motor_voltage_ = current_status.motor_voltage_;
-
}
-
-
Modified: pkg/trunk/mechanism/hardware_interface/include/hardware_interface/hardware_interface.h
===================================================================
--- pkg/trunk/mechanism/hardware_interface/include/hardware_interface/hardware_interface.h 2008-08-08 23:44:45 UTC (rev 2844)
+++ pkg/trunk/mechanism/hardware_interface/include/hardware_interface/hardware_interface.h 2008-08-08 23:50:01 UTC (rev 2845)
@@ -92,7 +92,7 @@
{
for (int i = 0; i < num_actuators; ++i)
{
- actuators_[i] = new Actuator();
+ actuators_.push_back(new Actuator());
}
}
~HardwareInterface()
Modified: pkg/trunk/mechanism/mechanism_control/src/mechanism_control.cpp
===================================================================
--- pkg/trunk/mechanism/mechanism_control/src/mechanism_control.cpp 2008-08-08 23:44:45 UTC (rev 2844)
+++ pkg/trunk/mechanism/mechanism_control/src/mechanism_control.cpp 2008-08-08 23:50:01 UTC (rev 2845)
@@ -64,10 +64,7 @@
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());
+ j->initXml(elt);
}
// Constructs the transmissions
@@ -84,20 +81,21 @@
if (joint_it == model_.joints_lookup_.end())
{
// TODO: report: The joint was not declared in the XML file
+ printf("Unable to locate joint: %s\n", elt->FirstChildElement("joint")->Attribute("name"));
continue;
}
if (actuator_it == model_.actuators_lookup_.end())
{
// TODO: report: The actuator was not registered with mechanism control.
+ printf("Unable to locate actuator: %s\n", elt->FirstChildElement("actuator")->Attribute("name"));
continue;
}
-
- Transmission *tr =
+ 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);
+ atof(elt->FirstChildElement("mechanicalReduction")->GetText()),
+ atof(elt->FirstChildElement("motorTorqueConstant")->GetText()),
+ atof(elt->FirstChildElement("pulsesPerRevolution")->GetText()));
+ model_.transmissions_.push_back(tr);
}
else
{
@@ -113,14 +111,13 @@
// 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
+ // Update all controllers
for (int i = 0; i < MAX_NUM_CONTROLLERS; ++i)
{
if (controllers_[i] != NULL)
Modified: pkg/trunk/mechanism/mechanism_model/CMakeLists.txt
===================================================================
--- pkg/trunk/mechanism/mechanism_model/CMakeLists.txt 2008-08-08 23:44:45 UTC (rev 2844)
+++ pkg/trunk/mechanism/mechanism_model/CMakeLists.txt 2008-08-08 23:50:01 UTC (rev 2845)
@@ -1,4 +1,4 @@
cmake_minimum_required(VERSION 2.6)
include(rosbuild)
rospack(mechanism_model)
-rospack_add_library(mechanism_model src/simple_transmission.cpp)
+rospack_add_library(mechanism_model src/simple_transmission.cpp src/joint.cpp)
Modified: pkg/trunk/mechanism/mechanism_model/include/mechanism_model/joint.h
===================================================================
--- pkg/trunk/mechanism/mechanism_model/include/mechanism_model/joint.h 2008-08-08 23:44:45 UTC (rev 2844)
+++ pkg/trunk/mechanism/mechanism_model/include/mechanism_model/joint.h 2008-08-08 23:50:01 UTC (rev 2845)
@@ -34,29 +34,28 @@
#ifndef JOINT_H
#define JOINT_H
+#include <tinyxml/tinyxml.h>
namespace mechanism {
class Joint{
public:
- void enforceLimits()
- {
- // TODO: enforce the limits so the joint operates safely
- }
+ void enforceLimits();
+ void initXml(TiXmlElement *elt);
char *name_;
int type_;
- //Update every cycle from input data
+ // Update every cycle from input data
bool initialized_;
double position_; // In radians
double velocity_;
double applied_effort_;
- //Written every cycle out to motor boards
+ // Written every cycle out to motor boards
double commanded_effort_;
- //Never changes
+ // Constants
double joint_limit_min_; // In radians
double joint_limit_max_; // In radians
double effort_limit_;
@@ -70,6 +69,7 @@
JOINT_CONTINUOUS,
JOINT_PRISMATIC,
JOINT_FIXED,
+ JOINT_PLANAR,
JOINT_TYPES_MAX
};
Modified: pkg/trunk/mechanism/mechanism_model/manifest.xml
===================================================================
--- pkg/trunk/mechanism/mechanism_model/manifest.xml 2008-08-08 23:44:45 UTC (rev 2844)
+++ pkg/trunk/mechanism/mechanism_model/manifest.xml 2008-08-08 23:50:01 UTC (rev 2845)
@@ -5,6 +5,7 @@
<license>BSD</license>
<depend package='hardware_interface'/>
<depend package="stl_utils" />
+<depend package="tinyxml" />
<url>http://pr.willowgarage.com</url>
<export>
<cpp cflags="-I${prefix}/include" lflags="-L${prefix}/lib -lmechanism_model -Wl,-rpath,${prefix}/lib"/>
Added: pkg/trunk/mechanism/mechanism_model/src/joint.cpp
===================================================================
--- pkg/trunk/mechanism/mechanism_model/src/joint.cpp (rev 0)
+++ pkg/trunk/mechanism/mechanism_model/src/joint.cpp 2008-08-08 23:50:01 UTC (rev 2845)
@@ -0,0 +1,73 @@
+/*********************************************************************
+ * 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 <map>
+#include <string>
+#include <mechanism_model/joint.h>
+
+using namespace std;
+using namespace mechanism;
+
+static const pair<string, int> types[] = {
+ pair<string, int>("none", JOINT_NONE),
+ pair<string, int>("revolute", JOINT_ROTARY),
+ pair<string, int>("prismatic", JOINT_PRISMATIC),
+ pair<string, int>("fixed", JOINT_FIXED),
+ pair<string, int>("planar", JOINT_PLANAR),
+};
+
+static map<string, int> g_type_map(types, types + sizeof(types)/sizeof(types[0]));
+
+void Joint::enforceLimits()
+{
+ // TODO: enforce the limits so the joint operates safely
+}
+
+void Joint::initXml(TiXmlElement *elt)
+{
+ TiXmlElement *min = elt->FirstChildElement("limitMin");
+ TiXmlElement *max = elt->FirstChildElement("limitMax");
+ joint_limit_min_ = min ? atof(min->GetText()) : 0;
+ joint_limit_max_ = max ? atof(max->GetText()) : 0;
+ effort_limit_ = atof(elt->FirstChildElement("effortLimit")->GetText());
+ velocity_limit_ = atof(elt->FirstChildElement("velocityLimit")->GetText());
+
+ type_ = g_type_map[elt->Attribute("type")];
+ // If type is revolute and limits aren't set, then it is continuous
+ if (type_ == JOINT_ROTARY && min == NULL && max == NULL)
+ {
+ type_ = JOINT_CONTINUOUS;
+ }
+}
+
Modified: pkg/trunk/mechanism/mechanism_model/src/simple_transmission.cpp
===================================================================
--- pkg/trunk/mechanism/mechanism_model/src/simple_transmission.cpp 2008-08-08 23:44:45 UTC (rev 2844)
+++ pkg/trunk/mechanism/mechanism_model/src/simple_transmission.cpp 2008-08-08 23:50:01 UTC (rev 2845)
@@ -67,6 +67,7 @@
void SimpleTransmission::propagateEffort()
{
actuator_->command_.current_ = joint_->commanded_effort_/(motor_torque_constant_ * mechanical_reduction_);
+ actuator_->command_.enable_ = true;
}
void SimpleTransmission::propagateEffortBackwards()
Modified: pkg/trunk/robot_control_loops/pr2_etherCAT/src/main.cpp
===================================================================
--- pkg/trunk/robot_control_loops/pr2_etherCAT/src/main.cpp 2008-08-08 23:44:45 UTC (rev 2844)
+++ pkg/trunk/robot_control_loops/pr2_etherCAT/src/main.cpp 2008-08-08 23:50:01 UTC (rev 2845)
@@ -37,7 +37,7 @@
#include <signal.h>
#include <sys/mman.h>
-#include <generic_controllers/joint_controller.h>
+#include <generic_controllers/joint_position_controller.h>
#include <mechanism_control/mechanism_control.h>
#include <ethercat_hardware/ethercat_hardware.h>
@@ -46,37 +46,39 @@
static int quit = 0;
static const int NSEC_PER_SEC = 1e+9;
-static void getTime(HardwareInterface *hw)
-{
- struct timespec now;
- clock_gettime(CLOCK_REALTIME, &now);
- hw->current_time_ = double(now.tv_nsec) / NSEC_PER_SEC + now.tv_sec;
-}
-
void *controlLoop(void *arg)
{
char **args = (char **) arg;
char *interface = args[1];
char *xml_file = args[2];
+ // Initialize the hardware inteface
+ EthercatHardware ec;
+ ec.init(interface);
+
+ // Create mechanism control
+ MechanismControl mc(ec.hw_);
+ //MechanismControlNode mcn(&mc);
+
+ // Load robot description
TiXmlDocument xml(xml_file);
xml.LoadFile();
TiXmlElement *root = xml.FirstChildElement("robot");
- EthercatHardware ec;
- ec.init(interface, root);
- getTime(ec.hw_);
+ // Register actuators with mechanism control
+ ec.initXml(root, mc);
- MechanismControl mc(ec.hw_);
- // TODO: register actuators with mechanism control
+ // Initialize mechanism control from robot description
mc.init(root);
- // TODO: register some controller types
- mc.registerControllerType("JointController", controller::JointController::create);
+ // Spawn controllers
+ // TODO what file does this come from?
+ // Can this be pushed down to mechanism control?
+ for (TiXmlElement *elt = root->FirstChildElement("controller"); elt ; elt = elt->NextSiblingElement("controller"))
+ {
+ mc.spawnController(elt->Attribute("type"), elt->Attribute("name"), elt);
+ }
- // TODO: spawn some controllers
- mc.spawnController("JointController", /*xml handle */NULL);
-
// Switch to hard real-time
int period = 1e+6; // 1 ms in nanoseconds
struct timespec tick;
@@ -88,8 +90,8 @@
while (!quit)
{
ec.update();
- getTime(ec.hw_);
mc.update();
+
tick.tv_nsec += period;
while (tick.tv_nsec >= NSEC_PER_SEC)
{
@@ -99,6 +101,7 @@
clock_nanosleep(CLOCK_REALTIME, TIMER_ABSTIME, &tick, NULL);
}
+ /* Shutdown all of the motors on exit */
for (unsigned int i = 0; i < ec.hw_->actuators_.size(); ++i)
{
ec.hw_->actuators_[i]->command_.enable_ = false;
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
