Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[6298] pkg/trunk
|
From: <stu...@us...> - 2008-11-06 00:29:53
|
Revision: 6298
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=6298&view=rev
Author: stuglaser
Date: 2008-11-06 00:29:49 +0000 (Thu, 06 Nov 2008)
Log Message:
-----------
Added a controller for calibrating the gripper
Modified Paths:
--------------
pkg/trunk/controllers/pr2_mechanism_controllers/CMakeLists.txt
pkg/trunk/controllers/robot_mechanism_controllers/CMakeLists.txt
pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_calibration_controller.h
pkg/trunk/controllers/robot_mechanism_controllers/src/joint_calibration_controller.cpp
pkg/trunk/mechanism/mechanism_model/include/mechanism_model/gripper_transmission.h
pkg/trunk/mechanism/mechanism_model/src/gripper_transmission.cpp
Added Paths:
-----------
pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/gripper_calibration_controller.h
pkg/trunk/controllers/pr2_mechanism_controllers/src/gripper_calibration_controller.cpp
Removed Paths:
-------------
pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_manual_calibration_controller.h
pkg/trunk/controllers/robot_mechanism_controllers/src/joint_manual_calibration_controller.cpp
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/CMakeLists.txt
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/CMakeLists.txt 2008-11-06 00:08:12 UTC (rev 6297)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/CMakeLists.txt 2008-11-06 00:29:49 UTC (rev 6298)
@@ -16,6 +16,7 @@
src/caster_controller.cpp
src/caster_calibration_controller.cpp
src/wrist_calibration_controller.cpp
+ src/gripper_calibration_controller.cpp
)
rospack_add_executable(testBaseController test/test_base_controller.cpp)
Copied: pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/gripper_calibration_controller.h (from rev 6297, pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_calibration_controller.h)
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/gripper_calibration_controller.h (rev 0)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/gripper_calibration_controller.h 2008-11-06 00:29:49 UTC (rev 6298)
@@ -0,0 +1,128 @@
+/*********************************************************************
+ * 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
+
+/*
+ Example XML:
+ <controller type="GripperCalibrationController">
+ <calibrate joint="upperarm_roll_right_joint"
+ actuator="upperarm_roll_right_act"
+ transmission="upperarm_roll_right_trans"
+ velocity="0.3" />
+ <pid p="15" i="0" d="0" iClamp="0" />
+ </controller>
+
+*/
+
+
+#include "mechanism_model/robot.h"
+#include "robot_mechanism_controllers/joint_velocity_controller.h"
+#include "misc_utils/realtime_publisher.h"
+#include "std_msgs/Empty.h"
+
+namespace controller
+{
+
+class GripperCalibrationController : public Controller
+{
+public:
+ GripperCalibrationController();
+ ~GripperCalibrationController();
+
+ virtual bool initXml(mechanism::RobotState *robot, TiXmlElement *config);
+
+ virtual void update();
+
+ bool calibrated() { return state_ == CALIBRATED; }
+ void beginCalibration() {
+ if (state_ == INITIALIZED)
+ state_ = BEGINNING;
+ }
+
+protected:
+
+ enum { INITIALIZED, BEGINNING, STARTING, CLOSING, CALIBRATED };
+ int state_;
+ int count_;
+
+ double search_velocity_;
+ Actuator *actuator_;
+ mechanism::JointState *joint_;
+
+ double init_time;
+
+ controller::JointVelocityController vc_; /** The joint velocity controller used to sweep the joint.*/
+};
+
+
+/***************************************************/
+/*! \class controller::GripperCalibrationControllerNode
+ \brief Joint Limit Controller ROS Node
+
+ This class starts and stops the initialization sequence
+
+*/
+/***************************************************/
+
+class GripperCalibrationControllerNode : public Controller
+{
+public:
+ /*!
+ * \brief Default Constructor
+ *
+ */
+ GripperCalibrationControllerNode();
+
+ /*!
+ * \brief Destructor
+ */
+ ~GripperCalibrationControllerNode();
+
+ void update();
+
+ bool initXml(mechanism::RobotState *robot, TiXmlElement *config);
+
+
+private:
+ mechanism::RobotState* robot_;
+ GripperCalibrationController c_;
+
+ double last_publish_time_;
+ misc_utils::RealtimePublisher<std_msgs::Empty> *pub_calibrated_;
+};
+
+}
+
+
Copied: pkg/trunk/controllers/pr2_mechanism_controllers/src/gripper_calibration_controller.cpp (from rev 6297, pkg/trunk/controllers/robot_mechanism_controllers/src/joint_calibration_controller.cpp)
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/gripper_calibration_controller.cpp (rev 0)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/gripper_calibration_controller.cpp 2008-11-06 00:29:49 UTC (rev 6298)
@@ -0,0 +1,192 @@
+/*********************************************************************
+ * 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 "pr2_mechanism_controllers/gripper_calibration_controller.h"
+#include <ros/time.h>
+
+using namespace std;
+using namespace controller;
+
+GripperCalibrationController::GripperCalibrationController()
+: state_(INITIALIZED), joint_(NULL)
+{
+}
+
+GripperCalibrationController::~GripperCalibrationController()
+{
+}
+
+bool GripperCalibrationController::initXml(mechanism::RobotState *robot, TiXmlElement *config)
+{
+ assert(robot);
+ assert(config);
+
+ TiXmlElement *cal = config->FirstChildElement("calibrate");
+ if (!cal)
+ {
+ std::cerr<<"GripperCalibrationController was not given calibration parameters"<<std::endl;
+ return false;
+ }
+
+ if(cal->QueryDoubleAttribute("velocity", &search_velocity_) != TIXML_SUCCESS)
+ {
+ std::cerr<<"Velocity value was not specified\n";
+ return false;
+ }
+
+ const char *joint_name = cal->Attribute("joint");
+ joint_ = joint_name ? robot->getJointState(joint_name) : NULL;
+ if (!joint_)
+ {
+ fprintf(stderr, "Error: GripperCalibrationController could not find joint \"%s\"\n",
+ joint_name);
+ return false;
+ }
+
+ const char *actuator_name = cal->Attribute("actuator");
+ actuator_ = actuator_name ? robot->model_->getActuator(actuator_name) : NULL;
+ if (!actuator_)
+ {
+ fprintf(stderr, "Error: GripperCalibrationController could not find actuator \"%s\"\n",
+ actuator_name);
+ return false;
+ }
+
+ control_toolbox::Pid pid;
+ TiXmlElement *pid_el = config->FirstChildElement("pid");
+ if (!pid_el)
+ {
+ fprintf(stderr, "Error: GripperCalibrationController was not given a pid element.\n");
+ return false;
+ }
+ if (!pid.initXml(pid_el))
+ return false;
+
+ if (!vc_.init(robot, joint_name, pid))
+ return false;
+
+ return true;
+}
+
+void GripperCalibrationController::update()
+{
+ assert(joint_);
+ assert(actuator_);
+
+ switch (state_)
+ {
+ case INITIALIZED:
+ state_ = BEGINNING;
+ return;
+ case BEGINNING:
+ count_ = 0;
+ joint_->calibrated_ = false;
+ actuator_->state_.zero_offset_ = 0.0;
+ vc_.setCommand(search_velocity_);
+ state_ = STARTING;
+ break;
+ case STARTING:
+ // Makes sure we start moving for a bit before checking if we've stopped.
+ if (++count_ > 500)
+ {
+ count_ = 0;
+ state_ = CLOSING;
+ }
+ break;
+ case CLOSING:
+ if (fabs(joint_->velocity_) < 0.001)
+ {
+ actuator_->state_.zero_offset_ = actuator_->state_.position_;
+ state_ = CALIBRATED;
+ joint_->calibrated_ = true;
+ vc_.setCommand(0);
+ }
+ break;
+ case CALIBRATED:
+ break;
+ }
+
+ if (state_ != CALIBRATED)
+ vc_.update();
+}
+
+
+ROS_REGISTER_CONTROLLER(GripperCalibrationControllerNode)
+
+GripperCalibrationControllerNode::GripperCalibrationControllerNode()
+: robot_(NULL)
+{
+}
+
+GripperCalibrationControllerNode::~GripperCalibrationControllerNode()
+{
+}
+
+void GripperCalibrationControllerNode::update()
+{
+ c_.update();
+
+ if (c_.calibrated())
+ {
+ if (last_publish_time_ + 0.5 < robot_->hw_->current_time_)
+ {
+ assert(pub_calibrated_);
+ if (pub_calibrated_->trylock())
+ {
+ last_publish_time_ = robot_->hw_->current_time_;
+ pub_calibrated_->unlockAndPublish();
+ }
+ }
+ }
+}
+
+bool GripperCalibrationControllerNode::initXml(mechanism::RobotState *robot, TiXmlElement *config)
+{
+ robot_ = robot;
+ ros::node *node = ros::node::instance();
+
+ std::string topic = config->Attribute("name") ? config->Attribute("name") : "";
+ if (topic == "")
+ {
+ fprintf(stderr, "No name given to GripperCalibrationController\n");
+ return false;
+ }
+
+ if (!c_.initXml(robot, config))
+ return false;
+
+ pub_calibrated_ = new misc_utils::RealtimePublisher<std_msgs::Empty>(topic + "/calibrated", 1);
+
+ return true;
+}
Modified: pkg/trunk/controllers/robot_mechanism_controllers/CMakeLists.txt
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/CMakeLists.txt 2008-11-06 00:08:12 UTC (rev 6297)
+++ pkg/trunk/controllers/robot_mechanism_controllers/CMakeLists.txt 2008-11-06 00:29:49 UTC (rev 6298)
@@ -16,6 +16,5 @@
src/joint_pd_controller.cpp
src/joint_calibration_controller.cpp
src/joint_blind_calibration_controller.cpp
- src/joint_manual_calibration_controller.cpp
- src/lqr_controller.cpp)
-
+ src/lqr_controller.cpp
+ )
\ No newline at end of file
Modified: pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_calibration_controller.h
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_calibration_controller.h 2008-11-06 00:08:12 UTC (rev 6297)
+++ pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_calibration_controller.h 2008-11-06 00:29:49 UTC (rev 6298)
@@ -58,11 +58,10 @@
/***************************************************/
-#include "joint_manual_calibration_controller.h"
+#include "mechanism_model/robot.h"
+#include "robot_mechanism_controllers/joint_velocity_controller.h"
#include "misc_utils/realtime_publisher.h"
#include "std_msgs/Empty.h"
-// Services
-#include <robot_mechanism_controllers/CalibrateJoint.h>
namespace controller
@@ -111,22 +110,9 @@
bool initXml(mechanism::RobotState *robot, TiXmlElement *config);
- /** DEPRECATED. Listen to /topic/calibrated instead
- *
- * \brief initializes the calibration procedure (blocking service)
- * This service starts the calibration sequence of the joint and waits to return until the calibration sequence is finished.
- *
- * @param req
- * @param resp
- * @return
- */
- bool calibrateCommand(robot_mechanism_controllers::CalibrateJoint::request &req,
- robot_mechanism_controllers::CalibrateJoint::response &resp);
-
private:
mechanism::RobotState* robot_;
JointCalibrationController c_;
- AdvertisedServiceGuard guard_calibrate_;
double last_publish_time_;
misc_utils::RealtimePublisher<std_msgs::Empty> *pub_calibrated_;
Deleted: pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_manual_calibration_controller.h
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_manual_calibration_controller.h 2008-11-06 00:08:12 UTC (rev 6297)
+++ pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_manual_calibration_controller.h 2008-11-06 00:29:49 UTC (rev 6298)
@@ -1,168 +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::JointCalibratonController
- \brief Joint Controller that finds zerop point
- \author Timothy Hunter <tjh...@wi...>
-
-
- This class moves the joint and reads the value of the clibration_reading_ field to find the zero position of the joint. Once these are determined, these values
- * are passed to the joint and enable the joint for the other controllers.
-
- */
-/***************************************************/
-
-
-#include <ros/node.h>
-#include <mechanism_model/controller.h>
-#include <robot_mechanism_controllers/joint_velocity_controller.h>
-#include <mechanism_model/robot.h>
-#include <hardware_interface/hardware_interface.h>
-
-// Services
-#include <robot_mechanism_controllers/CalibrateJoint.h>
-
-//FIXME: the editor messed up the indentation
-namespace controller
-{
-
- class JointManualCalibrationController : public Controller
- {
- public:
- /*!
- * \brief Default Constructor.
- *
- */
- JointManualCalibrationController();
-
- /*!
- * \brief Destructor.
- */
- virtual ~JointManualCalibrationController();
-
- /*!
- * \brief Functional way to initialize limits and gains.
- *
- */
- virtual bool initXml(mechanism::RobotState *robot, TiXmlElement *config);
-
- /*!
- * \brief Sets the joint in motion to find the reference position.
- *
- * There are two operating modes: manual and automatic.
- * The following algorithm is currently used in automatic mode: the controller sets a search direction for the reference point: positive direction if the current calibration reading is low, negative otherwie. It uses a velocity controller to move the joint in this direction and moves the joint until the calibration reading changes value. It then sets the offset filed in the related transmission accordingly.
- * In manual mode, the joint velocity controller is used to find the min and max limits by exploring the space at low speed.
- *
- */
- virtual void beginCalibration();
-
- virtual void endCalibration();
-
- bool getOffset(double & joint_angle);
-
- /** \brief Sets the offset of the joint
- */
- void setOffset(double joint_angle);
-
- /*!
- * \brief Issues commands to the joint. Should be called at regular intervals
- */
- virtual void update();
-
- bool calibrated() const { assert(joint_state_); return joint_state_->calibrated_; }
-
-
- protected:
-
- double offset(double act_pos, double joint_ref_pos);
-
- enum ControllerState {Stop,Search,Initialized,Begin,Idle} ;
-
- mechanism::Joint* joint_; /**< Joint we're controlling. */
- Actuator* actuator_; /** The actuator corresponding to the joint */
- mechanism::Robot *robot_; /**< Pointer to robot structure. */
- mechanism::Transmission * transmission_; /** The transmission associated to the actuator and the joint. */
- mechanism::JointState* joint_state_; /**< Joint we're controlling. */
-
- int state_; /** The current state of the controller*/
-
- double min_; // in actuator position
- double max_; // in actuator position
-
- ros::thread::mutex state_mutex_; /** Mutex locked during the calibration procedure to prevent lousy code from trying to find the offset while it is already searching. */
- };
-
-
- /***************************************************/
-/*! \class controller::JointCalibrationControllerNode
- \brief Joint Limit Controller ROS Node
-
- This class starts and stops the initialization sequence
-
- */
- /***************************************************/
-
- class JointManualCalibrationControllerNode : public Controller
- {
- public:
- /*!
- * \brief Default Constructor
- *
- */
- JointManualCalibrationControllerNode();
-
- /*!
- * \brief Destructor
- */
- virtual ~JointManualCalibrationControllerNode();
-
- void update();
-
- bool initXml(mechanism::RobotState *robot, TiXmlElement *config);
-
- // Services
- bool beginCalibrationCommand(robot_mechanism_controllers::CalibrateJoint::request &req,
- robot_mechanism_controllers::CalibrateJoint::response &resp);
- bool endCalibrationCommand(robot_mechanism_controllers::CalibrateJoint::request &req,
- robot_mechanism_controllers::CalibrateJoint::response &resp);
-
- private:
- JointManualCalibrationController *c_;
- };
-}
-
-
Modified: pkg/trunk/controllers/robot_mechanism_controllers/src/joint_calibration_controller.cpp
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/src/joint_calibration_controller.cpp 2008-11-06 00:08:12 UTC (rev 6297)
+++ pkg/trunk/controllers/robot_mechanism_controllers/src/joint_calibration_controller.cpp 2008-11-06 00:29:49 UTC (rev 6298)
@@ -41,7 +41,7 @@
//ROS_REGISTER_CONTROLLER(JointCalibrationController)
JointCalibrationController::JointCalibrationController()
-: state_(INITIALIZED)
+ : state_(INITIALIZED), actuator_(NULL), joint_(NULL), transmission_(NULL)
{
}
@@ -196,34 +196,20 @@
}
}
-
- bool JointCalibrationControllerNode::calibrateCommand(robot_mechanism_controllers::CalibrateJoint::request &req, robot_mechanism_controllers::CalibrateJoint::response &resp)
-{
- c_.beginCalibration();
- ros::Duration d=ros::Duration(0,1000000);
- while(!c_.calibrated())
- d.sleep();
- resp.offset = 0;
- return true;
-}
-
bool JointCalibrationControllerNode::initXml(mechanism::RobotState *robot, TiXmlElement *config)
{
assert(robot);
robot_ = robot;
ros::node *node = ros::node::instance();
- std::string topic = config->Attribute("topic") ? config->Attribute("topic") : "";
+ std::string topic = config->Attribute("name") ? config->Attribute("name") : "";
if (topic == "")
{
- fprintf(stderr, "No topic given to JointCalibrationController\n");
+ fprintf(stderr, "No name given to JointCalibrationController\n");
return false;
}
-
if (!c_.initXml(robot, config))
return false;
- node->advertise_service(topic + "/calibrate", &JointCalibrationControllerNode::calibrateCommand, this);
- guard_calibrate_.set(topic + "/calibrate");
pub_calibrated_ = new misc_utils::RealtimePublisher<std_msgs::Empty>(topic + "/calibrated", 1);
Deleted: pkg/trunk/controllers/robot_mechanism_controllers/src/joint_manual_calibration_controller.cpp
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/src/joint_manual_calibration_controller.cpp 2008-11-06 00:08:12 UTC (rev 6297)
+++ pkg/trunk/controllers/robot_mechanism_controllers/src/joint_manual_calibration_controller.cpp 2008-11-06 00:29:49 UTC (rev 6298)
@@ -1,243 +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 <robot_mechanism_controllers/joint_calibration_controller.h>
-#include <ros/time.h>
-
-using namespace std;
-using namespace controller;
-
-ROS_REGISTER_CONTROLLER(JointManualCalibrationController)
-
-JointManualCalibrationController::JointManualCalibrationController()
- : joint_(NULL),
- actuator_(NULL),
- robot_(NULL),
- transmission_(NULL),
- joint_state_(NULL),
- state_(Idle)
-{
- std::cout<<"JointManualCalibration created\n";
-}
-
-JointManualCalibrationController::~JointManualCalibrationController()
-{
- std::cout<<"JointManualCalibration destroyed\n";
-}
-
-bool JointManualCalibrationController::initXml(mechanism::RobotState *robot, TiXmlElement *config)
-{
- assert(robot);
- assert(config);
- robot_ = robot->model_;
-
- TiXmlElement *j = config->FirstChildElement("param");
- if (!j)
- {
- std::cerr<<"JointManualCalibrationController was not given parameters"<<std::endl;
- return false;
- }
-
- const char *joint_name = j->Attribute("joint_name");
- joint_ = joint_name ? robot_->getJoint(joint_name) : NULL;
- if (!joint_)
- {
- fprintf(stderr, "JointManualCalibrationController could not find joint named \"%s\"\n", joint_name);
- return false;
- }
-
- const int i = robot_->getJointIndex(joint_name);
- if(i<0)
- {
- std::cout<<"Could not find joint state\n";
- return false;
- }
- joint_state_ = &(robot->joint_states_[i]);
-
- const char *act_name = j->Attribute("actuator_name");
- actuator_ = act_name ? robot_->getActuator(act_name) : NULL;
- if(!actuator_)
- {
- std::cout<<"JointManualCalibrationController could not find an actuator called "<<act_name<<std::endl;
- return false;
- }
-
- const char *trans_name = j->Attribute("transmission_name");
- transmission_ = trans_name ? robot_->getTransmission(trans_name) : NULL;
- if(!transmission_)
- {
- std::cout<<"JointManualCalibrationController could not find a transmission called "<<trans_name<<std::endl;
- return false;
- }
-
- return true;
-}
-
-void JointManualCalibrationController::beginCalibration()
-{
- // Upward exploration
- if(state_mutex_.trylock())
- {
- state_ = Begin;
- std::cout<<"Starting calibration sequence "<<std::endl;
- }
- else
- std::cerr<<"JointManualCalibrationController"<<joint_->name_<<" : You tried to find the offset while it is already looking for it.\n";
-}
-
-void JointManualCalibrationController::endCalibration()
-{
- if(state_ != Search)
- {
- std::cerr<<"You tried to stop the calibration procedure while it is not searching\n";
- }
- state_ = Stop;
-}
-
-void JointManualCalibrationController::update()
-{
- assert(joint_state_);
- assert(actuator_);
-
- if(state_ == Begin)
- {
- joint_state_->calibrated_ = false;
- min_ = actuator_->state_.position_;
- max_ = actuator_->state_.position_;
- state_ = Search;
- }
-
- if(state_ == Search)
- {
- min_= std::min(min_, actuator_->state_.position_);
- max_= std::max(max_, actuator_->state_.position_);
- }
-
- if(state_ == Stop)
- {
- // Compute the offset:
- std::cout<<">>"<<min_<<" "<<max_<<std::endl;
- const double offset_min = offset(min_, joint_->joint_limit_min_);
- const double offset_max = offset(max_, joint_->joint_limit_max_);
- const double offset_avg = 0.5*(offset_min+offset_max);
- std::cout<<"Offset found: "<<offset_min<<'\t'<<offset_max<<'\t'<<offset_avg<<std::endl;
- actuator_->state_.zero_offset_=offset_avg;
- state_ = Initialized;
-
- }
-
- if(state_ == Initialized)
- {
- joint_state_->calibrated_ = true;
- state_mutex_.unlock();
- }
-}
-
-bool JointManualCalibrationController::getOffset(double & value)
-{
- value = actuator_->state_.zero_offset_;
- return state_ == Initialized;
-}
-
-double JointManualCalibrationController::offset(double act_pos, double joint_ref_pos)
-{
- std::cout<<"act_pos = "<<act_pos<<'\n';
- std::cout<<"ref_pos = "<<joint_ref_pos<<'\n';
- assert(transmission_);
-
- Actuator act;
- mechanism::JointState jState;
- std::vector<Actuator *> acts;
- std::vector<mechanism::JointState*> jStates;
- acts.push_back(&act);
- jStates.push_back(&jState);
-
- jState.position_ = joint_ref_pos;
-
- transmission_->propagatePositionBackwards(jStates, acts);
- std::cout<<"ref_pos (enc) = "<<act.state_.position_<<'\n';
- std::cout<<"computed offset = "<<act_pos - act.state_.position_<<'\n';
- return act_pos - act.state_.position_;
-}
-
-
-ROS_REGISTER_CONTROLLER(JointManualCalibrationControllerNode)
- JointManualCalibrationControllerNode::JointManualCalibrationControllerNode()
-{
- c_ = new JointManualCalibrationController();
-}
-
-JointManualCalibrationControllerNode::~JointManualCalibrationControllerNode()
-{
- delete c_;
-}
-
-void JointManualCalibrationControllerNode::update()
-{
- c_->update();
-}
-
- bool JointManualCalibrationControllerNode::beginCalibrationCommand(robot_mechanism_controllers::CalibrateJoint::request &req, robot_mechanism_controllers::CalibrateJoint::response &resp)
-{
- c_->beginCalibration();
- return true;
-}
-
- bool JointManualCalibrationControllerNode::endCalibrationCommand(robot_mechanism_controllers::CalibrateJoint::request &req, robot_mechanism_controllers::CalibrateJoint::response &resp)
-{
- c_->endCalibration();
- ros::Duration d=ros::Duration(0,1000000);
- while(!c_->calibrated())
- d.sleep();
- c_->getOffset(resp.offset);
- return true;
-}
-
-bool JointManualCalibrationControllerNode::initXml(mechanism::RobotState *robot, TiXmlElement *config)
-{
- ros::node *node = ros::node::instance();
-
- std::string topic = config->Attribute("topic") ? config->Attribute("topic") : "";
- if (topic == "")
- {
- fprintf(stderr, "No topic given to JointManualCalibrationController\n");
- return false;
- }
-
- if (!c_->initXml(robot, config))
- return false;
- node->advertise_service(topic + "/begin_manual_calibration", &JointManualCalibrationControllerNode::beginCalibrationCommand, this);
- node->advertise_service(topic + "/end_manual_calibration", &JointManualCalibrationControllerNode::endCalibrationCommand, this);
- return true;
-}
Modified: pkg/trunk/mechanism/mechanism_model/include/mechanism_model/gripper_transmission.h
===================================================================
--- pkg/trunk/mechanism/mechanism_model/include/mechanism_model/gripper_transmission.h 2008-11-06 00:08:12 UTC (rev 6297)
+++ pkg/trunk/mechanism/mechanism_model/include/mechanism_model/gripper_transmission.h 2008-11-06 00:29:49 UTC (rev 6298)
@@ -34,8 +34,6 @@
* <joint name="gripper_l_lower1_joint" reduction="-4" />
* <joint name="gripper_l_upper2_joint" reduction="-8" />
* <joint name="gripper_l_lower2_joint" reduction="8" />
- * <motorTorqueConstant>1</motorTorqueConstant>
- * <pulsesPerRevolution>90000</pulsesPerRevolution>
* <!-- GripTransmission uses a PID controller to keep the joint angles aligned in Gazebo -->
* <pid p="1.0" i="2.0" d="3.0" iClamp="2.0" /> <!-- Only needed for Gazebo -->
* </transmission>
@@ -69,9 +67,6 @@
std::vector<double> reductions_; // Mechanical reduction for each joint
std::vector<control_toolbox::Pid> pids_; // For keeping the joint angles aligned in Gazebo
-
- double motor_torque_constant_;
- double pulses_per_revolution_;
};
} // namespace mechanism
Modified: pkg/trunk/mechanism/mechanism_model/src/gripper_transmission.cpp
===================================================================
--- pkg/trunk/mechanism/mechanism_model/src/gripper_transmission.cpp 2008-11-06 00:08:12 UTC (rev 6297)
+++ pkg/trunk/mechanism/mechanism_model/src/gripper_transmission.cpp 2008-11-06 00:29:49 UTC (rev 6298)
@@ -51,6 +51,7 @@
fprintf(stderr, "GripperTransmission could not find actuator named \"%s\"\n", actuator_name);
return false;
}
+ robot->getActuator(actuator_name)->command_.enable_ = true;
actuator_names_.push_back(actuator_name);
for (TiXmlElement *j = config->FirstChildElement("joint"); j; j = j->NextSiblingElement("joint"))
@@ -82,9 +83,6 @@
pids_[i] = pid;
}
- motor_torque_constant_ = atof(config->FirstChildElement("motorTorqueConstant")->GetText()),
- pulses_per_revolution_ = atof(config->FirstChildElement("pulsesPerRevolution")->GetText());
-
return true;
}
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
