Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[12267] pkg/trunk
|
From: <stu...@us...> - 2009-03-07 01:13:58
|
Revision: 12267
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=12267&view=rev
Author: stuglaser
Date: 2009-03-07 01:13:47 +0000 (Sat, 07 Mar 2009)
Log Message:
-----------
During calibration, the joints move over the optical flag in one direction only.
Modified Paths:
--------------
pkg/trunk/controllers/robot_mechanism_controllers/CMakeLists.txt
pkg/trunk/mechanism/mechanism_bringup/scripts/calibrate_arm.py
pkg/trunk/robot_descriptions/pr2/pr2_defs/defs/arm_defs.xml
pkg/trunk/robot_descriptions/pr2/pr2_defs/defs/head_defs.xml
Added Paths:
-----------
pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_ud_calibration_controller.h
pkg/trunk/controllers/robot_mechanism_controllers/src/joint_ud_calibration_controller.cpp
Modified: pkg/trunk/controllers/robot_mechanism_controllers/CMakeLists.txt
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/CMakeLists.txt 2009-03-07 01:03:07 UTC (rev 12266)
+++ pkg/trunk/controllers/robot_mechanism_controllers/CMakeLists.txt 2009-03-07 01:13:47 UTC (rev 12267)
@@ -3,7 +3,7 @@
rospack(robot_mechanism_controllers)
genmsg()
gensrv()
-set(_srcs
+set(_srcs
src/plug_controller.cpp
src/cartesian_tff_controller.cpp
src/cartesian_trajectory_controller.cpp
@@ -23,6 +23,7 @@
src/joint_autotuner.cpp
src/joint_pd_controller.cpp
src/joint_calibration_controller.cpp
+ src/joint_ud_calibration_controller.cpp
src/joint_limit_calibration_controller.cpp
src/joint_blind_calibration_controller.cpp
src/lqr_controller.cpp
Added: pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_ud_calibration_controller.h
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_ud_calibration_controller.h (rev 0)
+++ pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_ud_calibration_controller.h 2009-03-07 01:13:47 UTC (rev 12267)
@@ -0,0 +1,100 @@
+/*********************************************************************
+ * 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
+
+#include "mechanism_model/robot.h"
+#include "robot_mechanism_controllers/joint_velocity_controller.h"
+#include "realtime_tools/realtime_publisher.h"
+#include "std_msgs/Empty.h"
+
+
+namespace controller
+{
+
+class JointUDCalibrationController : public controller::Controller
+{
+public:
+ JointUDCalibrationController();
+ virtual ~JointUDCalibrationController();
+
+ 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, MOVING_TO_LOW, MOVING_TO_HIGH, CALIBRATED };
+ int state_;
+ int countdown_;
+
+ double search_velocity_;
+ bool original_switch_state_;
+
+ Actuator *actuator_;
+ mechanism::JointState *joint_;
+ mechanism::Transmission *transmission_;
+
+ controller::JointVelocityController vc_;
+};
+
+
+class JointUDCalibrationControllerNode : public Controller
+{
+public:
+ JointUDCalibrationControllerNode();
+ ~JointUDCalibrationControllerNode();
+
+ void update();
+
+ bool initXml(mechanism::RobotState *robot, TiXmlElement *config);
+
+private:
+ mechanism::RobotState* robot_;
+ JointUDCalibrationController c_;
+
+ double last_publish_time_;
+ realtime_tools::RealtimePublisher<std_msgs::Empty> *pub_calibrated_;
+};
+
+}
+
+
Added: pkg/trunk/controllers/robot_mechanism_controllers/src/joint_ud_calibration_controller.cpp
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/src/joint_ud_calibration_controller.cpp (rev 0)
+++ pkg/trunk/controllers/robot_mechanism_controllers/src/joint_ud_calibration_controller.cpp 2009-03-07 01:13:47 UTC (rev 12267)
@@ -0,0 +1,237 @@
+/*********************************************************************
+ * 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_ud_calibration_controller.h>
+#include <ros/time.h>
+
+using namespace std;
+using namespace controller;
+
+//ROS_REGISTER_CONTROLLER(JointUDCalibrationController)
+
+JointUDCalibrationController::JointUDCalibrationController()
+ : state_(INITIALIZED), actuator_(NULL), joint_(NULL), transmission_(NULL)
+{
+}
+
+JointUDCalibrationController::~JointUDCalibrationController()
+{
+}
+
+bool JointUDCalibrationController::initXml(mechanism::RobotState *robot, TiXmlElement *config)
+{
+ assert(robot);
+ assert(config);
+
+ TiXmlElement *cal = config->FirstChildElement("calibrate");
+ if (!cal)
+ {
+ std::cerr<<"JointUDCalibrationController 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: JointUDCalibrationController 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: JointUDCalibrationController could not find actuator \"%s\"\n",
+ actuator_name);
+ return false;
+ }
+
+ const char *transmission_name = cal->Attribute("transmission");
+ transmission_ = transmission_name ? robot->model_->getTransmission(transmission_name) : NULL;
+ if (!transmission_)
+ {
+ fprintf(stderr, "Error: JointUDCalibrationController could not find transmission \"%s\"\n",
+ transmission_name);
+ return false;
+ }
+
+ control_toolbox::Pid pid;
+ TiXmlElement *pid_el = config->FirstChildElement("pid");
+ if (!pid_el)
+ {
+ fprintf(stderr, "Error: JointUDCalibrationController 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 JointUDCalibrationController::update()
+{
+ assert(joint_);
+ assert(actuator_);
+
+ switch(state_)
+ {
+ case INITIALIZED:
+ vc_.setCommand(0.0);
+ state_ = BEGINNING;
+ break;
+ case BEGINNING:
+ if (actuator_->state_.calibration_reading_)
+ state_ = MOVING_TO_LOW;
+ else
+ state_ = MOVING_TO_HIGH;
+ break;
+ case MOVING_TO_LOW:
+ vc_.setCommand(-search_velocity_);
+ if (!actuator_->state_.calibration_reading_)
+ {
+ if (--countdown_ <= 0)
+ state_ = MOVING_TO_HIGH;
+ }
+ else
+ countdown_ = 200;
+ break;
+ case MOVING_TO_HIGH: {
+ vc_.setCommand(search_velocity_);
+
+ if (actuator_->state_.calibration_reading_)
+ {
+ Actuator a;
+ mechanism::JointState j;
+ std::vector<Actuator*> fake_a;
+ std::vector<mechanism::JointState*> fake_j;
+ fake_a.push_back(&a);
+ fake_j.push_back(&j);
+
+ fake_a[0]->state_.position_ = actuator_->state_.last_calibration_rising_edge_;
+ transmission_->propagatePosition(fake_a, fake_j);
+
+
+
+ // Where was the joint when the optical switch triggered?
+ fake_a[0]->state_.position_ = actuator_->state_.last_calibration_rising_edge_;
+ transmission_->propagatePosition(fake_a, fake_j);
+
+ // What is the actuator position at the joint's zero?
+ fake_j[0]->position_ = fake_j[0]->position_ - joint_->joint_->reference_position_;
+ transmission_->propagatePositionBackwards(fake_j, fake_a);
+
+ actuator_->state_.zero_offset_ = fake_a[0]->state_.position_;
+ joint_->calibrated_ = true;
+
+ state_ = CALIBRATED;
+ vc_.setCommand(0.0);
+ }
+ break;
+ }
+ case CALIBRATED:
+ break;
+ }
+
+ if (state_ != CALIBRATED)
+ vc_.update();
+}
+
+
+ROS_REGISTER_CONTROLLER(JointUDCalibrationControllerNode)
+
+JointUDCalibrationControllerNode::JointUDCalibrationControllerNode()
+: robot_(NULL), last_publish_time_(0), pub_calibrated_(NULL)
+{
+}
+
+JointUDCalibrationControllerNode::~JointUDCalibrationControllerNode()
+{
+ if (pub_calibrated_)
+ {
+ std::string topic = pub_calibrated_->topic_;
+ delete pub_calibrated_;
+
+ // I think we're all tired of having the "cal" topics cluttering
+ // up rostopic and rosgraphviz.
+ ros::Node::instance()->unadvertise(topic);
+ }
+}
+
+void JointUDCalibrationControllerNode::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 JointUDCalibrationControllerNode::initXml(mechanism::RobotState *robot, TiXmlElement *config)
+{
+ assert(robot);
+ robot_ = robot;
+
+ std::string topic = config->Attribute("name") ? config->Attribute("name") : "";
+ if (topic == "")
+ {
+ fprintf(stderr, "No name given to JointUDCalibrationController\n");
+ return false;
+ }
+ if (!c_.initXml(robot, config))
+ return false;
+
+ pub_calibrated_ = new realtime_tools::RealtimePublisher<std_msgs::Empty>(topic + "/calibrated", 1);
+
+ return true;
+}
Modified: pkg/trunk/mechanism/mechanism_bringup/scripts/calibrate_arm.py
===================================================================
--- pkg/trunk/mechanism/mechanism_bringup/scripts/calibrate_arm.py 2009-03-07 01:03:07 UTC (rev 12266)
+++ pkg/trunk/mechanism/mechanism_bringup/scripts/calibrate_arm.py 2009-03-07 01:13:47 UTC (rev 12267)
@@ -111,7 +111,7 @@
def xml_for_cal(name, velocity, p, i, d, iClamp):
return '''\
<controller name=\"cal_%s" topic="cal_%s"\
-type="JointCalibrationControllerNode">\
+type="JointUDCalibrationControllerNode">\
<calibrate joint="%s_joint"\
actuator="%s_motor"\
transmission="%s_trans"\
Modified: pkg/trunk/robot_descriptions/pr2/pr2_defs/defs/arm_defs.xml
===================================================================
--- pkg/trunk/robot_descriptions/pr2/pr2_defs/defs/arm_defs.xml 2009-03-07 01:03:07 UTC (rev 12266)
+++ pkg/trunk/robot_descriptions/pr2/pr2_defs/defs/arm_defs.xml 2009-03-07 01:13:47 UTC (rev 12267)
@@ -706,7 +706,7 @@
<macro name="upper_arm_calibrator" params="side">
<controller name="${side}_cal_shoulder_pan" topic="${side}_cal_shoulder_pan"
- type="JointCalibrationControllerNode">
+ type="JointUDCalibrationControllerNode">
<calibrate joint="${side}_shoulder_pan_joint"
actuator="${side}_shoulder_pan_motor"
transmission="${side}_shoulder_pan_trans"
@@ -715,7 +715,7 @@
</controller>
<controller name="cal_shoulder_lift_${side}" topic="cal_shoulder_lift_${side}"
- type="JointCalibrationControllerNode">
+ type="JointUDCalibrationControllerNode">
<calibrate joint="${side}_shoulder_lift_joint"
actuator="${side}_shoulder_lift_motor"
transmission="${side}_shoulder_lift_trans"
@@ -724,7 +724,7 @@
</controller>
<controller name="cal_${side}_upper_arm_roll" topic="cal_${side}_upper_arm_roll"
- type="JointCalibrationControllerNode">
+ type="JointUDCalibrationControllerNode">
<calibrate joint="${side}_upper_arm_roll_joint"
actuator="${side}_upper_arm_roll_motor"
transmission="${side}_upper_arm_roll_trans"
@@ -733,7 +733,7 @@
</controller>
<controller name="cal_${side}_elbow_flex" topic="cal_${side}_elbow_flex"
- type="JointCalibrationControllerNode">
+ type="JointUDCalibrationControllerNode">
<calibrate joint="${side}_elbow_flex_joint"
actuator="${side}_elbow_flex_motor"
transmission="${side}_elbow_flex_trans"
@@ -744,7 +744,7 @@
<macro name="forearm_calibrator" params="side">
- <controller name="cal_${side}_forearm_roll" type="JointCalibrationControllerNode">
+ <controller name="cal_${side}_forearm_roll" type="JointUDCalibrationControllerNode">
<calibrate joint="${side}_forearm_roll_joint"
actuator="${side}_forearm_roll_motor"
transmission="${side}_forearm_roll_trans"
Modified: pkg/trunk/robot_descriptions/pr2/pr2_defs/defs/head_defs.xml
===================================================================
--- pkg/trunk/robot_descriptions/pr2/pr2_defs/defs/head_defs.xml 2009-03-07 01:03:07 UTC (rev 12266)
+++ pkg/trunk/robot_descriptions/pr2/pr2_defs/defs/head_defs.xml 2009-03-07 01:13:47 UTC (rev 12267)
@@ -391,7 +391,7 @@
<macro name="head_calibrator">
<controller name="cal_head_pan" topic="cal_head_pan"
- type="JointCalibrationControllerNode">
+ type="JointUDCalibrationControllerNode">
<calibrate joint="head_pan_joint"
actuator="head_pan_motor"
transmission="head_pan_trans"
@@ -400,7 +400,7 @@
</controller>
<controller name="cal_head_tilt" topic="cal_head_tilt"
- type="JointCalibrationControllerNode">
+ type="JointUDCalibrationControllerNode">
<calibrate joint="head_tilt_joint"
actuator="head_tilt_motor"
transmission="head_tilt_trans"
@@ -410,7 +410,7 @@
</macro>
<macro name="tilting_laser_calibrator" params="name">
- <controller name="cal_${name}" topic="cal_${name}" type="JointCalibrationControllerNode">
+ <controller name="cal_${name}" topic="cal_${name}" type="JointUDCalibrationControllerNode">
<calibrate joint="${name}_mount_joint"
actuator="${name}_mount_motor"
transmission="${name}_mount_trans"
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
