Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[6069] pkg/trunk
|
From: <stu...@us...> - 2008-10-31 01:00:45
|
Revision: 6069
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=6069&view=rev
Author: stuglaser
Date: 2008-10-31 01:00:39 +0000 (Fri, 31 Oct 2008)
Log Message:
-----------
The WristCalibrationController calibrates both joints of the wrist.
Modified Paths:
--------------
pkg/trunk/controllers/pr2_mechanism_controllers/CMakeLists.txt
pkg/trunk/mechanism/mechanism_model/include/mechanism_model/wrist_transmission.h
Added Paths:
-----------
pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/wrist_calibration_controller.h
pkg/trunk/controllers/pr2_mechanism_controllers/src/wrist_calibration_controller.cpp
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/CMakeLists.txt
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/CMakeLists.txt 2008-10-31 00:57:26 UTC (rev 6068)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/CMakeLists.txt 2008-10-31 01:00:39 UTC (rev 6069)
@@ -15,6 +15,7 @@
src/head_pan_tilt_controller.cpp
src/caster_controller.cpp
src/caster_calibration_controller.cpp
+ src/wrist_calibration_controller.cpp
)
rospack_add_executable(testBaseController test/test_base_controller.cpp)
Added: pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/wrist_calibration_controller.h
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/wrist_calibration_controller.h (rev 0)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/wrist_calibration_controller.h 2008-10-31 01:00:39 UTC (rev 6069)
@@ -0,0 +1,116 @@
+/*
+ * 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, 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.
+ */
+
+/*
+ Sample config:
+ <controller type="WristCalibrationController" name="cal_wrist">
+ <calibrate transmission="wrist_trans"
+ actuator_l="wrist_l_motor" actuator_r="wrist_r_motor"
+ flex_joint="wrist_flex_joint" roll_joint="wrist_roll_joint"
+ velocity="0.6" />
+ <pid p="3.0" i="0.2" d="0" iClamp="2.0" />
+ </controller>
+
+ * Author: Stuart Glaser
+ */
+
+#ifndef WRIST_CALIBRATION_CONTROLLER_H
+#define WRIST_CALIBRATION_CONTROLLER_H
+
+#include "robot_mechanism_controllers/joint_velocity_controller.h"
+#include "misc_utils/realtime_publisher.h"
+#include "mechanism_model/wrist_transmission.h"
+#include "std_msgs/Empty.h"
+
+namespace controller {
+
+class WristCalibrationController : public Controller
+{
+public:
+ WristCalibrationController();
+ ~WristCalibrationController();
+
+ virtual bool initXml(mechanism::RobotState *robot, TiXmlElement *config);
+
+
+ /*!
+ * \brief Issues commands to the joint. Should be called at regular intervals
+ */
+ virtual void update();
+
+ bool calibrated() { return state_ == CALIBRATED; }
+ void beginCalibration()
+ {
+ if (state_ == INITIALIZED)
+ state_ = BEGINNING;
+ }
+
+protected:
+
+ enum { INITIALIZED, BEGINNING, MOVING_FLEX, MOVING_ROLL, CALIBRATED };
+ int state_;
+
+ double search_velocity_;
+ bool original_switch_state_;
+
+ // Tracks the actuator positions for when the optical switch occurred.
+ double flex_switch_l_, flex_switch_r_;
+ double roll_switch_l_, roll_switch_r_;
+
+ double prev_actuator_l_position_, prev_actuator_r_position_;
+
+ Actuator *actuator_l_, *actuator_r_;
+ mechanism::JointState *flex_joint_, *roll_joint_;
+ mechanism::Transmission *transmission_;
+
+ controller::JointVelocityController vc_flex_, vc_roll_;
+};
+
+
+class WristCalibrationControllerNode : public Controller
+{
+public:
+ WristCalibrationControllerNode();
+ ~WristCalibrationControllerNode();
+
+ void update();
+
+ bool initXml(mechanism::RobotState *robot, TiXmlElement *config);
+
+private:
+ mechanism::RobotState *robot_;
+ WristCalibrationController c_;
+
+ double last_publish_time_;
+ misc_utils::RealtimePublisher<std_msgs::Empty> *pub_calibrated_;
+};
+
+}
+
+#endif
Added: pkg/trunk/controllers/pr2_mechanism_controllers/src/wrist_calibration_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/wrist_calibration_controller.cpp (rev 0)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/wrist_calibration_controller.cpp 2008-10-31 01:00:39 UTC (rev 6069)
@@ -0,0 +1,308 @@
+/*
+ * 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, 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.
+ */
+
+/*
+ * Author: Stuart Glaser
+ */
+
+#include "pr2_mechanism_controllers/wrist_calibration_controller.h"
+
+namespace controller {
+
+WristCalibrationController::WristCalibrationController()
+ : state_(INITIALIZED)
+{
+}
+
+WristCalibrationController::~WristCalibrationController()
+{
+}
+
+bool WristCalibrationController::initXml(mechanism::RobotState *robot, TiXmlElement *config)
+{
+ assert(robot);
+ assert(config);
+
+ TiXmlElement *cal = config->FirstChildElement("calibrate");
+ if (!cal)
+ {
+ std::cerr<<"WristCalibrationController 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 *flex_joint_name = cal->Attribute("flex_joint");
+ flex_joint_ = flex_joint_name ? robot->getJointState(flex_joint_name) : NULL;
+ if (!flex_joint_)
+ {
+ fprintf(stderr, "Error: WristCalibrationController could not find flex joint \"%s\"\n",
+ flex_joint_name);
+ return false;
+ }
+
+ const char *roll_joint_name = cal->Attribute("roll_joint");
+ roll_joint_ = roll_joint_name ? robot->getJointState(roll_joint_name) : NULL;
+ if (!roll_joint_)
+ {
+ fprintf(stderr, "Error: WristCalibrationController could not find roll_joint \"%s\"\n",
+ roll_joint_name);
+ return false;
+ }
+
+ const char *actuator_l_name = cal->Attribute("actuator_l");
+ actuator_l_ = actuator_l_name ? robot->model_->getActuator(actuator_l_name) : NULL;
+ if (!actuator_l_)
+ {
+ fprintf(stderr, "Error: WristCalibrationController could not find actuator_l \"%s\"\n",
+ actuator_l_name);
+ return false;
+ }
+
+ const char *actuator_r_name = cal->Attribute("actuator_r");
+ actuator_r_ = actuator_r_name ? robot->model_->getActuator(actuator_r_name) : NULL;
+ if (!actuator_r_)
+ {
+ fprintf(stderr, "Error: WristCalibrationController could not find actuator_r \"%s\"\n",
+ actuator_r_name);
+ return false;
+ }
+
+ const char *transmission_name = cal->Attribute("transmission");
+ transmission_ = transmission_name ? robot->model_->getTransmission(transmission_name) : NULL;
+ if (!transmission_)
+ {
+ fprintf(stderr, "Error: WristCalibrationController could not find transmission \"%s\"\n",
+ transmission_name);
+ return false;
+ }
+
+ control_toolbox::Pid pid;
+ TiXmlElement *p = config->FirstChildElement("pid");
+ if (p)
+ pid.initXml(p);
+ else
+ {
+ fprintf(stderr, "WristCalibrationController's config did not specify the default pid parameters.\n");
+ return false;
+ }
+
+ return
+ vc_flex_.init(robot, flex_joint_name, pid) &&
+ vc_roll_.init(robot, roll_joint_name, pid);
+}
+
+void WristCalibrationController::update()
+{
+ // Flex optical switch is connected to actuator_l
+ // Roll optical switch is connected to actuator_r
+
+ switch(state_)
+ {
+ case INITIALIZED:
+ vc_flex_.setCommand(0);
+ vc_roll_.setCommand(0);
+ state_ = BEGINNING;
+ break;
+ case BEGINNING:
+ original_switch_state_ = actuator_l_->state_.calibration_reading_;
+ vc_flex_.setCommand(original_switch_state_ ? -search_velocity_ : search_velocity_);
+ vc_roll_.setCommand(0);
+ state_ = MOVING_FLEX;
+ break;
+ case MOVING_FLEX: {
+ bool switch_state_ = actuator_l_->state_.calibration_reading_;
+ if (switch_state_ != original_switch_state_)
+ {
+ if (switch_state_ == true)
+ flex_switch_l_ = actuator_l_->state_.last_calibration_rising_edge_;
+ else
+ flex_switch_l_ = actuator_l_->state_.last_calibration_falling_edge_;
+
+ // But where was actuator_r at the transition? Unfortunately,
+ // actuator_r is not connected to the flex joint's optical
+ // switch, so we don't know directly. Instead, we estimate
+ // actuator_r's position based on the switch position of
+ // actuator_l.
+ double dl = actuator_l_->state_.position_ - prev_actuator_l_position_;
+ double dr = actuator_r_->state_.position_ - prev_actuator_r_position_;
+ double k = (flex_switch_l_ - prev_actuator_l_position_) / dl;
+ assert(0 <= k && k <= 1);
+ flex_switch_r_ = k * dr + prev_actuator_r_position_;
+
+ original_switch_state_ = actuator_r_->state_.calibration_reading_;
+ vc_flex_.setCommand(0);
+ vc_roll_.setCommand(original_switch_state_ ? -search_velocity_ : search_velocity_);
+ state_ = MOVING_ROLL;
+ }
+ break;
+ }
+ case MOVING_ROLL: {
+ bool switch_state_ = actuator_r_->state_.calibration_reading_;
+ if (switch_state_ != original_switch_state_)
+ {
+ if (switch_state_ == true)
+ flex_switch_r_ = actuator_r_->state_.last_calibration_rising_edge_;
+ else
+ flex_switch_r_ = actuator_r_->state_.last_calibration_falling_edge_;
+
+ // See corresponding comment above.
+ double dl = actuator_l_->state_.position_ - prev_actuator_l_position_;
+ double dr = actuator_r_->state_.position_ - prev_actuator_r_position_;
+ double k = (flex_switch_r_ - prev_actuator_r_position_) / dr;
+ assert(0 <= k && k <= 1);
+ flex_switch_l_ = k * dl + prev_actuator_l_position_;
+
+
+ //----------------------------------------------------------------------
+ // Calibration computation
+ //----------------------------------------------------------------------
+
+ // At this point, we know the actuator positions when the
+ // optical switches were hit. Now we compute the actuator
+ // positions when the joints should be at 0.
+
+ const int LEFT_MOTOR = mechanism::WristTransmission::LEFT_MOTOR;
+ const int RIGHT_MOTOR = mechanism::WristTransmission::RIGHT_MOTOR;
+ const int FLEX_JOINT = mechanism::WristTransmission::FLEX_JOINT;
+ const int ROLL_JOINT = mechanism::WristTransmission::ROLL_JOINT;
+
+ // Sets up the data structures for passing joint and actuator
+ // positions through the transmission.
+ Actuator fake_as_mem[2]; // This way we don't need to delete the objects later
+ mechanism::JointState fake_js_mem[2];
+ std::vector<Actuator*> fake_as;
+ std::vector<mechanism::JointState*> fake_js;
+ fake_as.push_back(&fake_as_mem[0]);
+ fake_as.push_back(&fake_as_mem[1]);
+ fake_js.push_back(&fake_js_mem[0]);
+ fake_js.push_back(&fake_js_mem[1]);
+
+ // Finds the (uncalibrated) joint position where the flex optical switch triggers
+ fake_as[LEFT_MOTOR]->state_.position_ = flex_switch_l_;
+ fake_as[RIGHT_MOTOR]->state_.position_ = flex_switch_r_;
+ transmission_->propagatePosition(fake_as, fake_js);
+ double flex_joint_switch_ = fake_js[FLEX_JOINT]->position_;
+
+ // Finds the (uncalibrated) joint position where the roll optical switch triggers
+ fake_as[LEFT_MOTOR]->state_.position_ = roll_switch_l_;
+ fake_as[RIGHT_MOTOR]->state_.position_ = roll_switch_r_;
+ transmission_->propagatePosition(fake_as, fake_js);
+ double roll_joint_switch_ = fake_js[ROLL_JOINT]->position_;
+
+ // Finds the (uncalibrated) joint position at the desired zero
+ fake_js[FLEX_JOINT]->position_ = flex_joint_switch_ - flex_joint_->joint_->reference_position_;
+ fake_js[ROLL_JOINT]->position_ = roll_joint_switch_ - roll_joint_->joint_->reference_position_;
+
+ // Determines the actuator zero position from the desired joint zero positions
+ transmission_->propagatePositionBackwards(fake_js, fake_as);
+ actuator_l_->state_.zero_offset_ = fake_as[LEFT_MOTOR]->state_.position_;
+ actuator_r_->state_.zero_offset_ = fake_as[RIGHT_MOTOR]->state_.position_;
+
+ flex_joint_->calibrated_ = true;
+ roll_joint_->calibrated_ = true;
+ state_ = CALIBRATED;
+
+ vc_flex_.setCommand(0);
+ vc_roll_.setCommand(0);
+ }
+
+ break;
+ }
+ case CALIBRATED:
+ break;
+ }
+
+ if (state_ != CALIBRATED)
+ {
+ vc_flex_.update();
+ vc_roll_.update();
+ }
+
+ prev_actuator_l_position_ = actuator_l_->state_.position_;
+ prev_actuator_r_position_ = actuator_r_->state_.position_;
+}
+
+
+ROS_REGISTER_CONTROLLER(WristCalibrationControllerNode)
+
+WristCalibrationControllerNode::WristCalibrationControllerNode()
+: last_publish_time_(0), pub_calibrated_(NULL)
+{
+}
+
+WristCalibrationControllerNode::~WristCalibrationControllerNode()
+{
+ if (pub_calibrated_)
+ delete pub_calibrated_;
+}
+
+void WristCalibrationControllerNode::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 WristCalibrationControllerNode::initXml(mechanism::RobotState *robot, TiXmlElement *config)
+{
+ assert(robot);
+ robot_ = robot;
+
+ std::string name = config->Attribute("name") ? config->Attribute("name") : "";
+ if (name == "")
+ {
+ fprintf(stderr, "No name given to WristCalibrationController\n");
+ return false;
+ }
+
+ if (!c_.initXml(robot, config))
+ return false;
+
+ pub_calibrated_ = new misc_utils::RealtimePublisher<std_msgs::Empty>(name + "/calibrated", 1);
+
+ return true;
+}
+
+}
Modified: pkg/trunk/mechanism/mechanism_model/include/mechanism_model/wrist_transmission.h
===================================================================
--- pkg/trunk/mechanism/mechanism_model/include/mechanism_model/wrist_transmission.h 2008-10-31 00:57:26 UTC (rev 6068)
+++ pkg/trunk/mechanism/mechanism_model/include/mechanism_model/wrist_transmission.h 2008-10-31 01:00:39 UTC (rev 6069)
@@ -33,15 +33,15 @@
*********************************************************************/
/*
* Author: Melonee Wise
-
+
example xml:
<robot name="wrist_trans">
- <joint name="right_wrist_flex_joint" type="revolute">
+ <joint name="right_wrist_flex_joint" type="revolute">
<limit min="-0.157" max="2.409" effort="5" velocity="5" />
<axis xyz="0 0 1" />
</joint>
- <joint name="right_wrist_roll_joint" type="continuous">
+ <joint name="right_wrist_roll_joint" type="continuous">
<limit min="0.0" max="0.0" effort="5" velocity="5" />
<axis xyz="0 0 1" />
</joint>
@@ -74,6 +74,11 @@
std::vector<double> mechanical_reduction_;
+ // Describes the order of the actuators and the joints in the arrays
+ // of names and of those passed to propagate*
+ enum { RIGHT_MOTOR, LEFT_MOTOR };
+ enum { FLEX_JOINT, ROLL_JOINT };
+
void propagatePosition(std::vector<Actuator*>&, std::vector<JointState*>&);
void propagatePositionBackwards(std::vector<JointState*>&, std::vector<Actuator*>&);
void propagateEffort(std::vector<JointState*>&, std::vector<Actuator*>&);
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
