Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[22437] pkg/trunk
|
From: <stu...@us...> - 2009-08-20 17:45:53
|
Revision: 22437
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=22437&view=rev
Author: stuglaser
Date: 2009-08-20 17:45:43 +0000 (Thu, 20 Aug 2009)
Log Message:
-----------
Moved qualification controller out of the controllers stack.
Added Paths:
-----------
pkg/trunk/sandbox/qualification_controllers/
pkg/trunk/sandbox/qualification_controllers/joint_qualification_controllers/
pkg/trunk/sandbox/qualification_controllers/joint_qualification_controllers/CMakeLists.txt
pkg/trunk/sandbox/qualification_controllers/joint_qualification_controllers/Makefile
pkg/trunk/sandbox/qualification_controllers/joint_qualification_controllers/controller_plugins.xml
pkg/trunk/sandbox/qualification_controllers/joint_qualification_controllers/include/
pkg/trunk/sandbox/qualification_controllers/joint_qualification_controllers/include/joint_qualification_controllers/
pkg/trunk/sandbox/qualification_controllers/joint_qualification_controllers/include/joint_qualification_controllers/checkout_controller.h
pkg/trunk/sandbox/qualification_controllers/joint_qualification_controllers/include/joint_qualification_controllers/hold_set_controller.h
pkg/trunk/sandbox/qualification_controllers/joint_qualification_controllers/include/joint_qualification_controllers/hysteresis_controller.h
pkg/trunk/sandbox/qualification_controllers/joint_qualification_controllers/include/joint_qualification_controllers/sine_sweep_controller.h
pkg/trunk/sandbox/qualification_controllers/joint_qualification_controllers/manifest.xml
pkg/trunk/sandbox/qualification_controllers/joint_qualification_controllers/msg/
pkg/trunk/sandbox/qualification_controllers/joint_qualification_controllers/msg/ActuatorData.msg
pkg/trunk/sandbox/qualification_controllers/joint_qualification_controllers/msg/HoldPositionData.msg
pkg/trunk/sandbox/qualification_controllers/joint_qualification_controllers/msg/HoldRunData.msg
pkg/trunk/sandbox/qualification_controllers/joint_qualification_controllers/msg/JointData.msg
pkg/trunk/sandbox/qualification_controllers/joint_qualification_controllers/msg/JointPositionData.msg
pkg/trunk/sandbox/qualification_controllers/joint_qualification_controllers/src/
pkg/trunk/sandbox/qualification_controllers/joint_qualification_controllers/src/checkout_controller.cpp
pkg/trunk/sandbox/qualification_controllers/joint_qualification_controllers/src/controller_manifest.cpp
pkg/trunk/sandbox/qualification_controllers/joint_qualification_controllers/src/hold_set_controller.cpp
pkg/trunk/sandbox/qualification_controllers/joint_qualification_controllers/src/hysteresis_controller.cpp
pkg/trunk/sandbox/qualification_controllers/joint_qualification_controllers/src/sine_sweep_controller.cpp
pkg/trunk/sandbox/qualification_controllers/joint_qualification_controllers/srv/
pkg/trunk/sandbox/qualification_controllers/joint_qualification_controllers/srv/HoldSetData.srv
pkg/trunk/sandbox/qualification_controllers/joint_qualification_controllers/srv/RobotData.srv
pkg/trunk/sandbox/qualification_controllers/joint_qualification_controllers/srv/TestData.srv
Removed Paths:
-------------
pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/CMakeLists.txt
pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/Makefile
pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/controller_plugins.xml
pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/include/joint_qualification_controllers/checkout_controller.h
pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/include/joint_qualification_controllers/hold_set_controller.h
pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/include/joint_qualification_controllers/hysteresis_controller.h
pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/include/joint_qualification_controllers/sine_sweep_controller.h
pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/manifest.xml
pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/msg/ActuatorData.msg
pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/msg/HoldPositionData.msg
pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/msg/HoldRunData.msg
pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/msg/JointData.msg
pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/msg/JointPositionData.msg
pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/src/checkout_controller.cpp
pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/src/controller_manifest.cpp
pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/src/hold_set_controller.cpp
pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/src/hysteresis_controller.cpp
pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/src/sine_sweep_controller.cpp
pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/srv/HoldSetData.srv
pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/srv/RobotData.srv
pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/srv/TestData.srv
Deleted: pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/CMakeLists.txt
===================================================================
--- pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/CMakeLists.txt 2009-08-20 17:43:35 UTC (rev 22436)
+++ pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/CMakeLists.txt 2009-08-20 17:45:43 UTC (rev 22437)
@@ -1,11 +0,0 @@
-cmake_minimum_required(VERSION 2.4.6)
-include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
-rospack(joint_qualification_controllers)
-genmsg()
-gensrv()
-rospack_add_library(joint_qualification_controllers
-src/controller_manifest.cpp
-src/hysteresis_controller.cpp
-src/sine_sweep_controller.cpp
-src/checkout_controller.cpp
-src/hold_set_controller.cpp)
Deleted: pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/Makefile
===================================================================
--- pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/Makefile 2009-08-20 17:43:35 UTC (rev 22436)
+++ pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/Makefile 2009-08-20 17:45:43 UTC (rev 22437)
@@ -1 +0,0 @@
-include $(shell rospack find mk)/cmake.mk
Deleted: pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/controller_plugins.xml
===================================================================
--- pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/controller_plugins.xml 2009-08-20 17:43:35 UTC (rev 22436)
+++ pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/controller_plugins.xml 2009-08-20 17:45:43 UTC (rev 22437)
@@ -1,6 +0,0 @@
-<library path="lib/libjoint_qualification_controllers">
- <class name="CheckoutController" type="CheckoutController" base_class_type="Controller" />
- <class name="HoldSetController" type="HoldSetController" base_class_type="Controller" />
- <class name="HysteresisController" type="HysteresisController" base_class_type="Controller" />
- <class name="SineSweepController" type="SineSweepController" base_class_type="Controller" />
-</library>
Deleted: pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/include/joint_qualification_controllers/checkout_controller.h
===================================================================
--- pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/include/joint_qualification_controllers/checkout_controller.h 2009-08-20 17:43:35 UTC (rev 22436)
+++ pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/include/joint_qualification_controllers/checkout_controller.h 2009-08-20 17:45:43 UTC (rev 22437)
@@ -1,133 +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.
- *********************************************************************/
-
-// Author: Kevin Watts
-
-#pragma once
-
-/***************************************************/
-/*! \class controller::CheckoutController
- \brief Checkout Controller
-
- This tests that all joints of a robot are calibrated
- and all actuators are enabled. It outputs a RobotData srv request
- to the /robot_checkout topic with relevant joint and actuator information.
-
-*/
-/***************************************************/
-
-#include <ros/ros.h>
-#include <string>
-#include <math.h>
-#include <joint_qualification_controllers/RobotData.h>
-#include <realtime_tools/realtime_srv_call.h>
-#include <controller_interface/controller.h>
-#include <boost/scoped_ptr.hpp>
-
-namespace controller
-{
-
-
-/***************************************************/
-/*! \class controller::CheckoutControllerNode
- \brief Checkout Controller
-
- */
-/***************************************************/
-
-class CheckoutController : public Controller
-{
-
-public:
- enum { STARTING, LISTENING, ANALYZING, DONE};
-
- CheckoutController();
- ~CheckoutController();
-
- /*!
- * \brief Functional way to initialize.
- * \param *robot The robot that is being controlled.
- * \param &n NodeHandle of mechanism control
- */
- bool init( mechanism::RobotState *robot, const ros::NodeHandle &n);
-
- /*!
- * \brief Called when controller is started or restarted
- */
- bool starting();
-
- /*!
- * \brief Checks joint, actuator status for calibrated and enabled.
- */
- void update();
-
- /*!
- * \brief Sends data, returns true if sent
- */
- bool sendData();
-
- /*!
- * \brief Perform the test analysis
- */
- void analysis(double time);
-
-
-private:
- mechanism::RobotState *robot_; /**< Pointer to robot structure. */
- double initial_time_; /**< Start time of the test. */
-
- double timeout_;
-
- joint_qualification_controllers::RobotData::Request robot_data_;
-
- int state_;
-
- int joint_count_;
- int actuator_count_;
-
- bool done() { return state_ == DONE; }
-
- bool data_sent_;
-
- double last_publish_time_;
-
- // RT service call
- boost::scoped_ptr<realtime_tools::RealtimeSrvCall<joint_qualification_controllers::RobotData::Request, joint_qualification_controllers::RobotData::Response> > call_service_;
-};
-
-
-
-}
-
-
Deleted: pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/include/joint_qualification_controllers/hold_set_controller.h
===================================================================
--- pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/include/joint_qualification_controllers/hold_set_controller.h 2009-08-20 17:43:35 UTC (rev 22436)
+++ pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/include/joint_qualification_controllers/hold_set_controller.h 2009-08-20 17:45:43 UTC (rev 22437)
@@ -1,202 +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::HoldSetController
- \brief Hold Set Controller
-
- This holds a joint in a set of locations, while
- dithering the joint and recording position and cmd.
-
-*/
-/***************************************************/
-
-
-#include <vector>
-#include <ros/ros.h>
-#include <math.h>
-#include <joint_qualification_controllers/HoldSetData.h>
-#include <realtime_tools/realtime_srv_call.h>
-#include <controller_interface/controller.h>
-#include <robot_mechanism_controllers/joint_position_controller.h>
-#include <control_toolbox/dither.h>
-
-#include <iostream>
-#include <string>
-#include <sstream>
-
-namespace controller
-{
-
-class HoldSetController : public Controller
-{
-
-public:
- enum { STARTING, SETTLING, DITHERING, PAUSING, DONE };
-
- HoldSetController();
- ~HoldSetController();
-
- /*!
- * \brief Functional way to initialize.
- * \param *robot The robot that is being controlled.
- * \param &n Node handle for parameters and services
- */
- bool init( mechanism::RobotState *robot, const ros::NodeHandle &n);
-
-
- /*!
- * \brief Issues commands to the joint. Should be called at regular intervals
- */
- void update();
-
- bool starting();
-
- bool sendData();
-
- bool done() { return state_ == DONE; }
-
- joint_qualification_controllers::HoldSetData::Request hold_set_data_;
-
-private:
- control_toolbox::Dither* lift_dither_;
- control_toolbox::Dither* flex_dither_;
-
- controller::JointPositionController* lift_controller_;
- controller::JointPositionController* flex_controller_;
-
- mechanism::JointState* flex_state_;
- mechanism::JointState* lift_state_;
-
- mechanism::RobotState *robot_; /**< Pointer to robot structure. */
-
- int starting_count_;
-
- int state_;
-
- double lift_cmd_, flex_cmd_;
-
-
- double initial_time_;
-
- double settle_time_;
- double start_time_;
- double dither_time_;
- double timeout_;
- double lift_min_, lift_max_, lift_delta_;
- double flex_min_, flex_max_, flex_delta_;
-
- int dither_count_;
-
- uint lift_index_;
- uint flex_index_;
-
- bool data_sent_;
-
- boost::scoped_ptr<realtime_tools::RealtimeSrvCall<joint_qualification_controllers::HoldSetData::Request, joint_qualification_controllers::HoldSetData::Response> > call_service_;
-
-
-};
-
-}
-
-
-/***************************************************/
-/*! \class controller::HoldSetController
- \brief Hold Set Controller
-
- This holds a joint at a set of positions, measuring effort
-
-<controller type="HoldSetControllerNode" name="cb_hold_set_controller">
-
- <controller name="shoulder_lift_controller" type="JointPositionController"><br>
- <dither dither_amp="0.25" />
- <joint name="r_shoulder_lift_joint" ><br>
- <pid p="1.5" d="0.1" i="0.3" iClamp="0.2" /><br>
- </joint><br>
- </controller><br>
-
- <controller name="elbow_flex_controller" type="JointPositionController"><br>
- <dither dither_amp="0.25" />
- <joint name="r_elbow_flex_joint" ><br>
- <pid p="0.8" d="0.05" i="0.1" iClamp="0.1" /><br>
- </joint><br>
- </controller><br>
-
- <controller_defaults settle_time="2.0" dither_time="1.0" timeout="60" />
-
- <hold_pt>
- <joint position="1.35" />
- <joint position="0.0" />
- </hold_pt>
-
- <hold_pt>
- <joint position="1.0" />
- <joint position="0.0" />
- </hold_pt>
-
- <hold_pt>
- <joint position="0.75" />
- <joint position="0.0" />
- </hold_pt>
-
- <hold_pt>
- <joint position="0.5" />
- <joint position="0.0" />
- </hold_pt>
-
- <hold_pt>
- <joint position="0.25" />
- <joint position="0.0" />
- </hold_pt>
-
- <hold_pt>
- <joint position="0.0" />
- <joint position="0.0" />
- </hold_pt>
-
- <hold_pt>
- <joint position="-0.25" />
- <joint position="0.0" />
- </hold_pt>
-
-
-</controller>
-
-*/
-/***************************************************/
-
-
Deleted: pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/include/joint_qualification_controllers/hysteresis_controller.h
===================================================================
--- pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/include/joint_qualification_controllers/hysteresis_controller.h 2009-08-20 17:43:35 UTC (rev 22436)
+++ pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/include/joint_qualification_controllers/hysteresis_controller.h 2009-08-20 17:45:43 UTC (rev 22437)
@@ -1,128 +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::HysteresisController
- \brief Hystersis Controller
-
- This tests the hysteresis of a joint using a
- velocity controller.
-
-*/
-/***************************************************/
-
-#include <ros/ros.h>
-#include <string>
-#include <math.h>
-#include <joint_qualification_controllers/TestData.h>
-#include <realtime_tools/realtime_srv_call.h>
-#include <controller_interface/controller.h>
-#include <robot_mechanism_controllers/joint_velocity_controller.h>
-#include <boost/scoped_ptr.hpp>
-
-namespace controller
-{
-
-class HysteresisController : public Controller
-{
-
-public:
- enum { STOPPED, STARTING, MOVING, ANALYZING, DONE};
-
- HysteresisController();
- ~HysteresisController();
-
- /*!
- * \brief Functional way to initialize.
- * \param *robot The robot that is being controlled.
- * \param &n NodeHandle of mechanism control
- */
- bool init(mechanism::RobotState *robot, const ros::NodeHandle &n);
-
- /*!
- * \brief Called when controller is started
- */
- bool starting();
-
- /*!
- * \brief Perform the test analysis
- */
- void analysis();
-
- /*!
- * \brief Issues commands to the joint to perform hysteresis test.
- */
- void update();
-
- /*!
- * \brief Sends data, returns true if sent
- */
- bool sendData();
-
- bool done() { return state_ == DONE; }
-
-
-private:
- joint_qualification_controllers::TestData::Request test_data_;
-
- mechanism::JointState *joint_; /**< Joint we're controlling. */
- mechanism::RobotState *robot_; /**< Pointer to robot structure. */
- controller::JointVelocityController *velocity_controller_; /**< The velocity controller for the hysteresis test. */
- double velocity_; /**< Velocity during the test. */
- double max_effort_; /**< Maximum allowable effort. */
- double initial_time_; /**< Start time of the test. */
- double initial_position_;
- int count_;
- int loop_count_;
- bool complete;
- bool start;
-
- double timeout_;
-
- int state_;
- int starting_count;
-
- bool data_sent_;
-
- double last_publish_time_;
-
- // RT service call
- boost::scoped_ptr<realtime_tools::RealtimeSrvCall<joint_qualification_controllers::TestData::Request, joint_qualification_controllers::TestData::Response> > call_service_;
-
-};
-}
-
-
Deleted: pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/include/joint_qualification_controllers/sine_sweep_controller.h
===================================================================
--- pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/include/joint_qualification_controllers/sine_sweep_controller.h 2009-08-20 17:43:35 UTC (rev 22436)
+++ pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/include/joint_qualification_controllers/sine_sweep_controller.h 2009-08-20 17:45:43 UTC (rev 22437)
@@ -1,106 +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::SineSweepController
- \brief Sine Sweep Controller
-
- This class basically applies a sine sweep to the joint.
-*/
-/***************************************************/
-
-
-#include <ros/ros.h>
-#include <math.h>
-#include <joint_qualification_controllers/TestData.h>
-#include <realtime_tools/realtime_srv_call.h>
-#include <controller_interface/controller.h>
-#include <control_toolbox/sine_sweep.h>
-#include <boost/scoped_ptr.hpp>
-
-
-namespace controller
-{
-
-class SineSweepController : public Controller
-{
-public:
- /*!
- * \brief Default Constructor of the SineSweepController class.
- *
- */
- SineSweepController();
-
- /*!
- * \brief Destructor of the SineSweepController class.
- */
- ~SineSweepController();
-
- /*!
- * \brief Functional way to initialize.
- * \param *robot The robot that is being controlled.
- * \param &n NodeHandle of mechanism control
- */
- bool init(mechanism::RobotState *robot, const ros::NodeHandle &n);
-
- bool starting();
-
- void analysis();
- void update();
-
- bool sendData();
-
- bool done() { return done_ == 1; }
-
- joint_qualification_controllers::TestData::Request test_data_;
-
-private:
- mechanism::JointState *joint_state_; /**< Joint we're controlling. */
- mechanism::RobotState *robot_; /**< Pointer to robot structure. */
- control_toolbox::SineSweep *sweep_; /**< Sine sweep. */
- double duration_; /**< Duration of the sweep. */
- double initial_time_; /**< Start time of the sweep. */
- int count_;
- bool done_;
-
- bool data_sent_;
-
- boost::scoped_ptr<realtime_tools::RealtimeSrvCall<joint_qualification_controllers::TestData::Request, joint_qualification_controllers::TestData::Response> >call_service_;
-
-};
-}
-
-
Deleted: pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/manifest.xml
===================================================================
--- pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/manifest.xml 2009-08-20 17:43:35 UTC (rev 22436)
+++ pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/manifest.xml 2009-08-20 17:45:43 UTC (rev 22437)
@@ -1,25 +0,0 @@
-<package>
- <description brief='Joint Qualification Controllers'>
- </description>
- <author> Melonee Wise, Kevin Watts</author>
- <license>BSD</license>
- <review status="unreviewed" notes=""/>
- <depend package="controller_interface" />
- <depend package="mechanism_model" />
- <depend package="wg_robot_description_parser" />
- <depend package="realtime_tools" />
- <depend package="control_toolbox" />
- <depend package="roscpp" />
- <depend package="robot_mechanism_controllers" />
- <depend package="mechanism_control" />
- <depend package="pluginlib" />
- <url>http://pr.willowgarage.com/joint_qualification_controllers</url>
- <rosdep name="wxwidgets"/>
- <rosdep name="wxpython"/>
- <repository>http://pr.willowgarage.com/repos</repository>
- <export>
- <cpp cflags='-I${prefix}/include -I${prefix}/msg/cpp -I${prefix}/srv/cpp' lflags='-Wl,-rpath,${prefix}/lib -L${prefix}/lib -ljoint_qualification_controllers'/>
- <controller_interface plugin="${prefix}/controller_plugins.xml" />
- </export>
-
-</package>
Deleted: pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/msg/ActuatorData.msg
===================================================================
--- pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/msg/ActuatorData.msg 2009-08-20 17:43:35 UTC (rev 22436)
+++ pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/msg/ActuatorData.msg 2009-08-20 17:45:43 UTC (rev 22437)
@@ -1,4 +0,0 @@
-int16 index
-string name
-int16 id
-byte enabled
Deleted: pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/msg/HoldPositionData.msg
===================================================================
--- pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/msg/HoldPositionData.msg 2009-08-20 17:43:35 UTC (rev 22436)
+++ pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/msg/HoldPositionData.msg 2009-08-20 17:45:43 UTC (rev 22437)
@@ -1,3 +0,0 @@
-float32 flex_position
-JointPositionData lift_hold
-JointPositionData flex_hold
Deleted: pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/msg/HoldRunData.msg
===================================================================
--- pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/msg/HoldRunData.msg 2009-08-20 17:43:35 UTC (rev 22436)
+++ pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/msg/HoldRunData.msg 2009-08-20 17:45:43 UTC (rev 22437)
@@ -1,2 +0,0 @@
-float32 lift_position
-HoldPositionData[] flex_data # Same lift position, diff flex
\ No newline at end of file
Deleted: pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/msg/JointData.msg
===================================================================
--- pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/msg/JointData.msg 2009-08-20 17:43:35 UTC (rev 22436)
+++ pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/msg/JointData.msg 2009-08-20 17:45:43 UTC (rev 22437)
@@ -1,6 +0,0 @@
-int16 index
-string name
-byte is_cal
-byte has_safety
-string type
-
Deleted: pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/msg/JointPositionData.msg
===================================================================
--- pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/msg/JointPositionData.msg 2009-08-20 17:43:35 UTC (rev 22436)
+++ pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/msg/JointPositionData.msg 2009-08-20 17:45:43 UTC (rev 22437)
@@ -1,5 +0,0 @@
-float32[] time
-float32[] position
-float32[] velocity
-float32[] cmd
-float32[] effort
\ No newline at end of file
Deleted: pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/src/checkout_controller.cpp
===================================================================
--- pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/src/checkout_controller.cpp 2009-08-20 17:43:35 UTC (rev 22436)
+++ pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/src/checkout_controller.cpp 2009-08-20 17:45:43 UTC (rev 22437)
@@ -1,267 +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 <joint_qualification_controllers/checkout_controller.h>
-
-using namespace std;
-using namespace controller;
-
-ROS_REGISTER_CONTROLLER(CheckoutController)
-
-CheckoutController::CheckoutController()
-: robot_(NULL),
- data_sent_(false),
- call_service_(NULL)
-{
- robot_data_.test_time = 0;
- robot_data_.num_joints = 0;
- robot_data_.num_actuators = 0;
-
- state_ = STARTING;
- joint_count_ = 0;
- actuator_count_ = 0;
-
- timeout_ = 30.0;
-}
-
-CheckoutController::~CheckoutController()
-{
-}
-
-bool CheckoutController::init(mechanism::RobotState *robot, const ros::NodeHandle &n)
-{
- assert(robot);
- robot_ = robot;
-
- joint_count_ = robot_->joint_states_.size();
- robot_data_.num_joints = joint_count_;
-
- robot_data_.joint_data.resize(joint_count_);
-
- for (int i = 0; i < joint_count_; i++)
- {
- mechanism::Joint *joint = robot_->joint_states_[i].joint_;
-
- robot_data_.joint_data[i].index = i;
- robot_data_.joint_data[i].name = joint->name_;
- robot_data_.joint_data[i].is_cal = 0;
- robot_data_.joint_data[i].has_safety = joint->has_safety_limits_;
-
- // Assign type strings based on type
- switch (joint->type_)
- {
- case (mechanism::JOINT_NONE):
- robot_data_.joint_data[i].type = "None";
- break;
- case (mechanism::JOINT_ROTARY):
- robot_data_.joint_data[i].type = "Rotary";
- break;
- case (mechanism::JOINT_CONTINUOUS):
- robot_data_.joint_data[i].type = "Continuous";
- break;
- case (mechanism::JOINT_PRISMATIC):
- robot_data_.joint_data[i].type = "Prismatic";
- break;
- case (mechanism::JOINT_FIXED):
- robot_data_.joint_data[i].type = "Fixed";
- break;
- case (mechanism::JOINT_PLANAR):
- robot_data_.joint_data[i].type = "Planar";
- break;
- case (mechanism::JOINT_TYPES_MAX):
- robot_data_.joint_data[i].type = "Types max";
- break;
- default:
- robot_data_.joint_data[i].type = "No type given!";
- break;
- }
- }
-
- // Assign actuators
- actuator_count_ = robot_->hw_->actuators_.size();
- robot_data_.num_actuators = actuator_count_;
- robot_data_.actuator_data.resize(actuator_count_);
-
- for (int i = 0; i < actuator_count_; i++)
- {
- Actuator *actuator = robot_->hw_->actuators_[i];
-
- robot_data_.actuator_data[i].index = i;
- robot_data_.actuator_data[i].name = actuator->name_;
- robot_data_.actuator_data[i].id = actuator->state_.device_id_;
- robot_data_.actuator_data[i].enabled = 0;
- }
-
- n.param("timeout", timeout_, 30.0);
-
- initial_time_ = robot_->hw_->current_time_;
-
- call_service_.reset(new realtime_tools::RealtimeSrvCall<joint_qualification_controllers::RobotData::Request, joint_qualification_controllers::RobotData::Response>(n, "/robot_checkout"));
-
- return true;
-}
-
-bool CheckoutController::starting()
-{
- initial_time_ = robot_->hw_->current_time_;
-
- data_sent_ = false;
-
- return true;
-}
-
-void CheckoutController::update()
-{
- double time = robot_->hw_->current_time_;
- bool cal = false;
- bool enabled = false;
-
- // Timeout check.
- if (time - initial_time_ > timeout_ && state_ != ANALYZING && state_ != DONE)
- {
- analysis(0);
- state_ = DONE;
- }
-
- switch (state_)
- {
- case STARTING:
- initial_time_ = robot_->hw_->current_time_;
- state_ = LISTENING;
- break;
- case LISTENING:
- {
- cal = true;
- for (int i = 0; i < joint_count_; ++i)
- {
- // Check for caster wheel joints and fingers
- // Wheel joints and fingers don't calibrate, so don't wait for them
- if (cal && (robot_->joint_states_[i].joint_->name_.find("wheel_joint") != string::npos))
- continue;
-
-
- if (cal && (robot_->joint_states_[i].joint_->name_.find("finger") != string::npos))
- continue;
-
-
- // Base joint is a dummy joint used to set up visualization
- if (cal && robot_->joint_states_[i].joint_->name_ == "base_joint")
- continue;
-
- if (!robot_->joint_states_[i].calibrated_)
- {
- cal = false;
- break;
- }
- }
-
- enabled = true;
- for (int i = 0; i < actuator_count_; i++)
- {
- if (!robot_->hw_->actuators_[i]->state_.is_enabled_)
- {
- enabled = false;
- break;
- }
- }
-
- if (cal && enabled)
- state_ = ANALYZING;
-
- break;
- }
- case ANALYZING:
- analysis(time - initial_time_);
- state_ = DONE;
- break;
- case DONE:
- if (!data_sent_)
- data_sent_ = sendData();
-
-
- break;
- }
-
-}
-
-bool CheckoutController::sendData()
-{
- if (call_service_->trylock())
- {
- joint_qualification_controllers::RobotData::Request *out = &call_service_->srv_req_;
- out->test_time = robot_data_.test_time;
- out->num_joints = robot_data_.num_joints;
- out->num_actuators = robot_data_.num_actuators;
-
- out->joint_data.resize(robot_data_.num_joints);
- out->actuator_data.resize(robot_data_.num_actuators);
-
- for (int i = 0; i < joint_count_; i++)
- {
- out->joint_data[i].index = robot_data_.joint_data[i].index;
- out->joint_data[i].name = robot_data_.joint_data[i].name;
- out->joint_data[i].is_cal = robot_data_.joint_data[i].is_cal;
- out->joint_data[i].has_safety = robot_data_.joint_data[i].has_safety;
- out->joint_data[i].type = robot_data_.joint_data[i].type;
- }
-
- for (int i = 0; i < actuator_count_; i++)
- {
- out->actuator_data[i].index = robot_data_.actuator_data[i].index;
- out->actuator_data[i].name = robot_data_.actuator_data[i].name;
- out->actuator_data[i].id = robot_data_.actuator_data[i].id;
- out->actuator_data[i].enabled = robot_data_.actuator_data[i].enabled;
- }
-
- call_service_->unlockAndCall();
-
- return true;
- }
- return false;
-}
-
-void CheckoutController::analysis(double time)
-{
- robot_data_.test_time = time;
-
- for (int i = 0; i < joint_count_; i++)
- robot_data_.joint_data[i].is_cal = robot_->joint_states_[i].calibrated_;
-
- for (int i = 0; i < actuator_count_; i++)
- robot_data_.actuator_data[i].enabled = robot_->hw_->actuators_[i]->state_.is_enabled_;
-
- return;
-}
-
-
Deleted: pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/src/controller_manifest.cpp
===================================================================
--- pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/src/controller_manifest.cpp 2009-08-20 17:43:35 UTC (rev 22436)
+++ pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/src/controller_manifest.cpp 2009-08-20 17:45:43 UTC (rev 22437)
@@ -1,43 +0,0 @@
-/*
- * Copyright (c) 2009, 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, 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 "pluginlib/class_list_macros.h"
-#include "controller_interface/controller.h"
-
-#include "joint_qualification_controllers/checkout_controller.h"
-#include "joint_qualification_controllers/hold_set_controller.h"
-#include "joint_qualification_controllers/hysteresis_controller.h"
-#include "joint_qualification_controllers/sine_sweep_controller.h"
-
-using namespace controller;
-
-PLUGINLIB_REGISTER_CLASS(CheckoutController, CheckoutController, Controller)
-PLUGINLIB_REGISTER_CLASS(HoldSetController, HoldSetController, Controller)
-PLUGINLIB_REGISTER_CLASS(HysteresisController, HysteresisController, Controller)
-PLUGINLIB_REGISTER_CLASS(SineSweepController, SineSweepController, Controller)
Deleted: pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/src/hold_set_controller.cpp
===================================================================
--- pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/src/hold_set_controller.cpp 2009-08-20 17:43:35 UTC (rev 22436)
+++ pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/src/hold_set_controller.cpp 2009-08-20 17:45:43 UTC (rev 22437)
@@ -1,325 +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.
- *********************************************************************/
-
-/* Author: Kevin Watts */
-
-///\todo Rename to CounterbalanceTestController
-
-#include <joint_qualification_controllers/hold_set_controller.h>
-
-using namespace std;
-using namespace controller;
-
-ROS_REGISTER_CONTROLLER(HoldSetController)
-
-HoldSetController::HoldSetController()
-: robot_(NULL),
- initial_time_(0.0),
- start_time_(0.0),
- lift_min_(0.0),
- lift_max_(0.0),
- flex_min_(0.0),
- flex_max_(0.0),
- flex_delta_(0.0)
-{
- dither_time_ = 0.0;
- lift_cmd_ = 0.0;
- lift_delta_ = 0.0;
- flex_cmd_ = 0.0;
- timeout_ = 120;
- lift_index_ = 0;
- flex_index_ = 0;
-
- hold_set_data_.arg_name.resize(10);
- hold_set_data_.arg_value.resize(10);
- hold_set_data_.arg_name[0] = "Settle Time";
- hold_set_data_.arg_name[1] = "Start Time";
- hold_set_data_.arg_name[2] = "Dither Time";
- hold_set_data_.arg_name[3] = "Timeout";
- hold_set_data_.arg_name[4] = "Lift Min";
- hold_set_data_.arg_name[5] = "Lift Max";
- hold_set_data_.arg_name[6] = "Lift Delta";
- hold_set_data_.arg_name[7] = "Flex Min";
- hold_set_data_.arg_name[8] = "Flex Max";
- hold_set_data_.arg_name[9] = "Flex Delta";
-
- ///\todo Need PID's for lift, flex
-
- state_ = STARTING;
-}
-
-HoldSetController::~HoldSetController()
-{
-}
-
-bool HoldSetController::init(mechanism::RobotState *robot, const ros::NodeHandle &n)
-{
- assert(robot);
- robot_ = robot;
-
- // Lift joint
- std::string lift_name;
- if (!n.getParam("lift_name", lift_name)){
- ROS_ERROR("CounterbalanceTestController: No lift joint name found on parameter namespace: %s)",
- n.getNamespace().c_str());
- return false;
- }
- if (!(lift_state_ = robot->getJointState(lift_name)))
- {
- ROS_ERROR("CounterbalanceTestController could not find lift joint named \"%s\"\n", lift_name.c_str());
- return false;
- }
- hold_set_data_.lift_name = lift_name;
-
- lift_controller_ = new JointPositionController();
- if (!lift_controller_->init(robot, ros::NodeHandle(n, "lift"))) return false;
-
- // Flex joint
- std::string flex_name;
- if (!n.getParam("flex_name", flex_name)){
- ROS_ERROR("CounterbalanceTestController: No flex joint name found on parameter namespace: %s)",
- n.getNamespace().c_str());
- return false;
- }
- if (!(flex_state_ = robot->getJointState(flex_name)))
- {
- ROS_ERROR("CounterbalanceTestController could not find flex joint named \"%s\"\n", flex_name.c_str());
- return false;
- }
- hold_set_data_.flex_name = flex_name;
-
- flex_controller_ = new JointPositionController();
- if (!flex_controller_->init(robot, ros::NodeHandle(n, "flex"))) return false;
-
- // Initialize dithers
- lift_dither_ = new control_toolbox::Dither(100.0);
- if (!lift_dither_->init(ros::NodeHandle(n, "lift")))
- return false;
-
- flex_dither_ = new control_toolbox::Dither(200.0);
- if (!flex_dither_->init(ros::NodeHandle(n, "flex")))
- return false;
-
- // Lift range
- if (!n.getParam("lift/min", lift_min_)){
- ROS_ERROR("CounterbalanceTestController: No min lift position found on parameter namespace: %s)",
- n.getNamespace().c_str());
- return false;
- }
- if (!n.getParam("lift/max", lift_max_)){
- ROS_ERROR("CounterbalanceTestController: No max lift position found on parameter namespace: %s)",
- n.getNamespace().c_str());
- return false;
- }
- if (!n.getParam("lift/delta", lift_delta_)){
- ROS_ERROR("CounterbalanceTestController: No lift delta found on parameter namespace: %s)",
- n.getNamespace().c_str());
- return false;
- }
-
- // Flex range
- if (!n.getParam("flex/min", flex_max_)){
- ROS_ERROR("CounterbalanceTestController: No min flex position found on parameter namespace: %s)",
- n.getNamespace().c_str());
- return false;
- }
- if (!n.getParam("flex/max", flex_min_)){
- ROS_ERROR("CounterbalanceTestController: No max flex position found on parameter namespace: %s)",
- n.getNamespace().c_str());
- return false;
- }
- if (!n.getParam("flex/delta", flex_delta_)){
- ROS_ERROR("CounterbalanceTestController: No flex delta found on parameter namespace: %s)",
- n.getNamespace().c_str());
- return false;
- }
-
- // Setting controller defaults
- if (!n.getParam("settle_time", settle_time_)){
- ROS_ERROR("CounterbalanceTestController: No settle time found on parameter namespace: %s)",
- n.getNamespace().c_str());
- return false;
- }
- if (!n.getParam("dither_time", dither_time_)){
- ROS_ERROR("CounterbalanceTestController: No dither time found on parameter namespace: %s)",
- n.getNamespace().c_str());
- return false;
- }
- if (!n.getParam("timeout", timeout_)){
- ROS_ERROR("CounterbalanceTestController: No timeout found on parameter namespace: %s)",
- n.getNamespace().c_str());
- return false;
- }
-
- lift_cmd_ = lift_min_;
- flex_cmd_ = flex_min_ - flex_delta_;
-
- return true;
-}
-
-bool HoldSetController::starting()
-{
- initial_time_ = robot_->hw_->current_time_;
- return true;
-}
-
-void HoldSetController::update()
-{
- // wait until the joints are calibrated to start
- if (!flex_state_->calibrated_ || !lift_state_->calibrated_)
- return;
-
- double time = robot_->hw_->current_time_;
-
- if (time - initial_time_ > timeout_ && state_ != DONE)
- {
- ROS_ERROR("CounterbalanceTestController timed out during test. Timeout: %f.", timeout_);
- state_ = DONE;
- }
-
- lift_controller_->update();
- flex_controller_->update();
-
- switch (state_)
- {
- case STARTING:
- {
- ROS_INFO("Starting");
-
- // Set the flex and lift commands
- // If
- flex_cmd_ += flex_delta_;
- flex_index_++;
- // Move to next lift position, reset flex
- if (flex_cmd_ > flex_max_)
- {
- flex_cmd_ = flex_min_ - flex_delta_;
- lift_cmd_ += lift_delta_;
- lift_index_++;
-
- // We're done after we finished the lifts
- if (lift_cmd_ > lift_max_)
- {
- state_ = DONE;
- break;
- }
- hold_set_data_.lift_data.resize(lift_index_ + 1);
- hold_set_data_.lift_data[lift_index_].lift_position = lift_cmd_;
- }
- else
- {
- hold_set_data_.lift_data[lift_index_].flex_data.resize(flex_index_ + 1);
- hold_set_data_.lift_data[lift_index_].flex_data[flex_index_].flex_position = flex_cmd_;
- }
-
- // Set controllers
- lift_controller_->setCommand(lift_cmd_);
- flex_controller_->setCommand(flex_cmd_);
-
- start_time_ = time;
- state_ = SETTLING;
- break;
- }
- case SETTLING:
- {
- if (time - start_time_ > settle_time_)
- {
- state_ = DITHERING;
- start_time_ = time;
- ROS_INFO("Dithering!");
- }
-
- break;
- }
- case DITHERING:
- {
- // Add dither
- lift_state_->commanded_effort_ += lift_dither_->update();
- flex_state_->commanded_effort_ += flex_dither_->update();
-
- // Lift
- hold_set_data_.lift_data[lift_index_].flex_data[flex_index_].lift_hold.time.push_back(time - start_time_);
- hold_set_data_.lift_data[lift_index_].flex_data[flex_index_].lift_hold.position.push_back(lift_state_->position_);
- hold_set_data_.lift_data[lift_index_].flex_data[flex_index_].lift_hold.velocity.push_back(lift_state_->velocity_);
- hold_set_data_.lift_data[lift_index_].flex_data[flex_index_].lift_hold.cmd.push_back(lift_state_->commanded_effort_);
- hold_set_data_.lift_data[lift_index_].flex_data[flex_index_].lift_hold.effort.push_back(lift_state_->applied_effort_);
-
- // Flex
- hold_set_data_.lift_data[lift_index_].flex_data[flex_index_].flex_hold.time.push_back(time - start_time_);
- hold_set_data_.lift_data[lift_index_].flex_data[flex_index_].flex_hold.position.push_back(flex_state_->position_);
- hold_set_data_.lift_data[lift_index_].flex_data[flex_index_].flex_hold.velocity.push_back(flex_state_->velocity_);
- hold_set_data_.lift_data[lift_index_].flex_data[flex_index_].flex_hold.cmd.push_back(flex_state_->commanded_effort_);
- hold_set_data_.lift_data[lift_index_].flex_data[flex_index_].flex_hold.effort.push_back(flex_state_->applied_effort_);
-
- if (time - start_time_ > dither_time_)
- {
- state_ = STARTING;
- }
- break;
- }
- case DONE:
- {
- if (!data_sent_)
- data_sent_ = sendData();
-
- break;
- }
-
- }
-}
-
-bool HoldSetController::sendData()
-{
- if (call_service_->trylock())
- {
- ROS_INFO("Calling results service!");
-
- // Copy data and send
- joint_qualification_controllers::HoldSetData::Request *out = &call_service_->srv_req_;
-
- out->lift_name = hold_set_data_.lift_name;
- out->flex_name = hold_set_data_.flex_name;
- out->lift_amplitude = hold_set_data_.lift_amplitude;
- out->flex_amplitude = hold_set_data_.flex_amplitude;
-
- out->arg_name = hold_set_data_.arg_name;
- out->arg_value = hold_set_data_.arg_value;
-
- out->lift_data = hold_set_data_.lift_data;
- call_service_->unlockAndCall();
- return true;
- }
- return false;
-}
-
Deleted: pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/src/hysteresis_controller.cpp
===================================================================
--- pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/src/hysteresis_controller.cpp 2009-08-20 17:43:35 UTC (rev 22436)
+++ pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/src/hysteresis_controller.cpp 2009-08-20 17:45:43 UTC (rev 22437)
@@ -1,313 +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 <joint_qualification_controllers/hysteresis_controller.h>
-
-#define MAX_DATA_POINTS 120000
-
-using namespace std;
-using namespace controller;
-
-ROS_REGISTER_CONTROLLER(HysteresisController)
-
-HysteresisController::HysteresisController()
-: joint_(NULL),
- robot_(NULL),
- data_sent_(false),
- call_service_(NULL)
-{
- test_data_.test_name ="hysteresis";
- test_data_.joint_name = "default joint";
- test_data_.time.resize(MAX_DATA_POINTS);
- test_data_.cmd.resize(MAX_DATA_POINTS);
- test_data_.effort.resize(MAX_DATA_POINTS);
- test_data_.position.resize(MAX_DATA_POINTS);
- test_data_.velocity.resize(MAX_DATA_POINTS);
-
- test_data_.arg_name.resize(11);
- test_data_.arg_value.resize(11);
- test_data_.arg_name[0] = "Min. Expected Effort";
- test_data_.arg_name[1] = "Max. Expected Effort";
- test_data_.arg_name[2] = "Minimum Position";
- test_data_.arg_name[3] = "Minimum Position";
- test_data_.arg_name[4] = "Velocity";
- test_data_.arg_name[5] = "Timeout";
- test_data_.arg_name[6] = "Max. Allowed Effort";
- test_data_.arg_name[7] = "P Gain";
- test_data_.arg_name[8] = "I Gain";
- test_data_.arg_name[9] = "D Gain";
- test_data_.arg_name[10] = "I-Clamp";
-
- state_ = STOPPED;
- starting_count = 0;
- velocity_ = 0;
- initial_time_ = 0;
- max_effort_ = 0;
- complete = false;
- start = true;
- loop_count_ = 0;
- count_ = 0;
-
- // Assume 1KHz update rate
- timeout_ = MAX_DATA_POINTS / 1000;
-}
-
-HysteresisController::~HysteresisController()
-{
-}
-
-bool HysteresisController::init( mechanism::RobotState *robot, const ros::NodeHandle &n)
-{
- assert(robot);
- robot_ = robot;
-
- std::string name;
- if (!n.getParam("velocity_controller/joint", name)){
- ROS_ERROR("Hysteresis Controller: No joint name found on parameter namespace: %s)",
- n.getNamespace().c_str());
- return false;
- }
- if (!(joint_ = robot->getJointState(name)))
- {
- ROS_ERROR("HysteresisController could not find joint named \"%s\"\n", name.c_str());
- return false;
- }
-
- if (!n.getParam("velocity", velocity_)){
- ROS_ERROR("Hysteresis Controller: No velocity found on parameter namespace: %s)",
- n.getNamespace().c_str());
- return false;
- }
-
- if (!n.getParam("max_effort", max_effort_)){
- ROS_ERROR("Hysteresis Controller: No max effort found on parameter namespace: %s)",
- n.getNamespace().c_str());
- return false;
- }
-
- double min_expected, max_expected, max_pos, min_pos;
-
- if (!n.getParam("min_expected", min_expected)){
- ROS_ERROR("Hysteresis Controller: No min expected effort found on parameter namespace: %s)",
...
[truncated message content] |
