Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[5411] pkg/trunk
|
From: <stu...@us...> - 2008-10-15 22:33:48
|
Revision: 5411
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=5411&view=rev
Author: stuglaser
Date: 2008-10-15 22:33:39 +0000 (Wed, 15 Oct 2008)
Log Message:
-----------
Calibration of joints with optical sensors is now asynchronous
Modified Paths:
--------------
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/robot.h
Added Paths:
-----------
pkg/trunk/robot_descriptions/wg_robot_description/one_armed_bandit/calibrate.py
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-10-15 22:33:12 UTC (rev 5410)
+++ pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_calibration_controller.h 2008-10-15 22:33:39 UTC (rev 5411)
@@ -59,7 +59,8 @@
#include "joint_manual_calibration_controller.h"
-
+#include "misc_utils/realtime_publisher.h"
+#include "std_msgs/Empty.h"
// Services
#include <robot_mechanism_controllers/CalibrateJoint.h>
@@ -70,30 +71,14 @@
class JointCalibrationController : public controller::Controller
{
public:
- /*!
- * \brief Default Constructor.
- *
- */
JointCalibrationController();
-
- /*!
- * \brief Destructor.
- */
virtual ~JointCalibrationController();
- /*!
- * \brief Functional way to initialize limits and gains.
- *
- */
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_ == STOPPED; }
+ bool calibrated() { return state_ == CALIBRATED; }
void beginCalibration()
{
if (state_ == INITIALIZED)
@@ -102,7 +87,7 @@
protected:
- enum { INITIALIZED, BEGINNING, MOVING, STOPPED };
+ enum { INITIALIZED, BEGINNING, MOVING, CALIBRATED };
int state_;
double search_velocity_;
@@ -116,29 +101,19 @@
};
-/** @class controller::JointControllerCalibrationNode
- * @\brief ROS interface for a joint calibration controller
- * This class is a wrapper around the calibrateCmd service call and it should be its only use
- */
class JointCalibrationControllerNode : public Controller
{
public:
- /*!
- * \brief Default Constructor
- *
- */
JointCalibrationControllerNode();
-
- /*!
- * \brief Destructor
- */
~JointCalibrationControllerNode();
void update();
bool initXml(mechanism::RobotState *robot, TiXmlElement *config);
- /** \brief initializes the calibration procedure (blocking service)
+ /** 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
@@ -149,8 +124,12 @@
robot_mechanism_controllers::CalibrateJoint::response &resp);
private:
- JointCalibrationController *c_;
+ mechanism::RobotState* robot_;
+ JointCalibrationController c_;
AdvertisedServiceGuard guard_calibrate_;
+
+ double last_publish_time_;
+ misc_utils::RealtimePublisher<std_msgs::Empty> *pub_calibrated_;
};
}
Modified: pkg/trunk/controllers/robot_mechanism_controllers/src/joint_calibration_controller.cpp
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/src/joint_calibration_controller.cpp 2008-10-15 22:33:12 UTC (rev 5410)
+++ pkg/trunk/controllers/robot_mechanism_controllers/src/joint_calibration_controller.cpp 2008-10-15 22:33:39 UTC (rev 5411)
@@ -38,7 +38,7 @@
using namespace std;
using namespace controller;
-ROS_REGISTER_CONTROLLER(JointCalibrationController)
+//ROS_REGISTER_CONTROLLER(JointCalibrationController)
JointCalibrationController::JointCalibrationController()
: state_(INITIALIZED)
@@ -119,6 +119,7 @@
{
case INITIALIZED:
vc_.setCommand(0.0);
+ state_ = BEGINNING;
break;
case BEGINNING:
original_switch_state_ = actuator_->state_.calibration_reading_ > 0.5;
@@ -147,42 +148,55 @@
actuator_->state_.zero_offset_ = fake_a[0]->state_.position_;
- state_ = STOPPED;
+ state_ = CALIBRATED;
+ vc_.setCommand(0.0);
}
break;
}
- case STOPPED:
- vc_.setCommand(0.0);
+ case CALIBRATED:
break;
}
- vc_.update();
+ if (state_ != CALIBRATED)
+ vc_.update();
}
ROS_REGISTER_CONTROLLER(JointCalibrationControllerNode)
JointCalibrationControllerNode::JointCalibrationControllerNode()
+: robot_(NULL), last_publish_time_(0), pub_calibrated_(NULL)
{
- c_ = new JointCalibrationController();
}
JointCalibrationControllerNode::~JointCalibrationControllerNode()
{
- delete c_;
}
void JointCalibrationControllerNode::update()
{
- c_->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 JointCalibrationControllerNode::calibrateCommand(robot_mechanism_controllers::CalibrateJoint::request &req, robot_mechanism_controllers::CalibrateJoint::response &resp)
{
- c_->beginCalibration();
+ c_.beginCalibration();
ros::Duration d=ros::Duration(0,1000000);
- while(!c_->calibrated())
+ while(!c_.calibrated())
d.sleep();
resp.offset = 0;
return true;
@@ -190,6 +204,8 @@
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") : "";
@@ -199,10 +215,12 @@
return false;
}
- if (!c_->initXml(robot, config))
+ 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);
+
return true;
}
Modified: pkg/trunk/mechanism/mechanism_model/include/mechanism_model/robot.h
===================================================================
--- pkg/trunk/mechanism/mechanism_model/include/mechanism_model/robot.h 2008-10-15 22:33:12 UTC (rev 5410)
+++ pkg/trunk/mechanism/mechanism_model/include/mechanism_model/robot.h 2008-10-15 22:33:39 UTC (rev 5411)
@@ -36,19 +36,10 @@
* The robot model tracks the state of the robot.
*
* State path:
- * +---------------+ +--------+
- * Actuators --> | Transmissions | --> Joints --> | Chains | --> Links
- * +---------------+ +--------+
+ * +---------------+
+ * Actuators --> | Transmissions | --> Joints --> Links
+ * +---------------+
*
- * The actuators, joints, and links, hold the state information. The
- * actuators hold the encoder info, the joints hold the joint angles
- * and velocities, and the links hold the frame transforms of the body
- * segments.
- *
- * The transmissions and chains are for propagating the state through
- * the model, and they themselves do not hold any information on the
- * robot's current state.
- *
* Author: Stuart Glaser
*/
Added: pkg/trunk/robot_descriptions/wg_robot_description/one_armed_bandit/calibrate.py
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description/one_armed_bandit/calibrate.py (rev 0)
+++ pkg/trunk/robot_descriptions/wg_robot_description/one_armed_bandit/calibrate.py 2008-10-15 22:33:39 UTC (rev 5411)
@@ -0,0 +1,185 @@
+#!/usr/bin/python
+# 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: Stuart Glaser
+
+import rostools
+import copy
+import threading
+import sys
+from time import sleep
+
+# Loads interface with the robot.
+rostools.update_path('teleop_robot')
+import rospy
+from std_msgs.msg import *
+from robot_srvs.srv import *
+from robot_mechanism_controllers.srv import *
+
+def slurp(filename):
+ f = open(filename)
+ stuff = f.read()
+ f.close()
+ return stuff
+
+rospy.wait_for_service('spawn_controller')
+spawn_controller = rospy.ServiceProxy('spawn_controller', SpawnController)
+kill_controller = rospy.ServiceProxy('kill_controller', KillController)
+
+
+def calibrate_optically(config):
+ resp = spawn_controller(config)
+ if len(resp.ok) != 1 or not resp.ok[0]:
+ print "FAIL"
+ return
+ name = resp.name[0]
+ do_calibration = rospy.ServiceProxy("/%s/calibrate" % name, CalibrateJoint)
+ do_calibration()
+ kill_controller(name)
+ print "Calibrated"
+
+def calibrate_manually(config):
+ resp = spawn_controller(config)
+ if len(resp.ok) != 1 or not resp.ok[0]:
+ print "FAIL"
+ return
+ name = resp.name[0]
+ begin = rospy.ServiceProxy("/%s/begin_manual_calibration" % name, CalibrateJoint)
+ end = rospy.ServiceProxy("/%s/end_manual_calibration" % name, CalibrateJoint)
+ begin()
+ print "Move the joint to the limits, and then hit enter"
+ raw_input()
+ end()
+ kill_controller(name)
+ print "Calibrated manually"
+
+# Hits the joint stops
+def calibrate_blindly(config):
+ resp = spawn_controller(config)
+ if len(resp.ok) != 1 or not resp.ok[0]:
+ print "FAIL"
+ return
+ name = resp.name[0]
+ do_calibration = rospy.ServiceProxy("/%s/calibrate" % name, CalibrateJoint)
+ do_calibration()
+ kill_controller(name)
+ print "Calibrated"
+
+def calibrate(config):
+ # Spawns the controllers
+ resp = spawn_controller(config)
+
+ # Accumulates the list of spawned controllers
+ launched = []
+ try:
+ for i in range(len(resp.ok)):
+ if not resp.ok[i]:
+ print "Failed: %s" % resp.name[i]
+ else:
+ launched.append(resp.name[i])
+ print "Launched: %s" % ', '.join(launched)
+
+ # Sets up callbacks for calibration completion
+ waiting_for = launched[:]
+ def calibrated(msg, name): # Somewhat not thread-safe
+ if name in waiting_for:
+ waiting_for.remove(name)
+ for name in waiting_for:
+ rospy.Subscriber("/%s/calibrated" % name, Empty, calibrated, name)
+
+ # Waits until all the controllers have calibrated
+ while waiting_for:
+ print "Waiting for: %s" % ', '.join(waiting_for)
+ sleep(0.5)
+ finally:
+ [kill_controller(name) for name in launched]
+
+
+rospy.init_node('calibration', anonymous=True)
+
+
+calibrate('''
+<controllers>
+
+ <controller name="cal_shoulder_pan" topic="cal_shoulder_pan" type="JointCalibrationControllerNode">
+ <calibrate joint="shoulder_pan_right_joint"
+ actuator="shoulder_pan_right_motor"
+ transmission="shoulder_pan_right_trans"
+ velocity="0.6" />
+ <pid p="7" i="0.5" d="0" iClamp="1.0" />
+ </controller>
+
+ <controller name="cal_shoulder_pitch" topic="cal_shoulder_pitch" type="JointCalibrationControllerNode">
+ <calibrate joint="shoulder_pitch_right_joint"
+ actuator="shoulder_pitch_right_motor"
+ transmission="shoulder_pitch_right_trans"
+ velocity="0.6" />
+ <pid p="7" i="0.5" d="0" iClamp="1.0" />
+ </controller>
+
+ <controller name="cal_laser_tilt" topic="cal_laser_tilt" type="JointCalibrationControllerNode">
+ <calibrate joint="tilt_laser_mount_joint"
+ actuator="tilt_laser_motor"
+ transmission="tilt_laser_mount_trans"
+ velocity="-0.6" />
+ <pid p=".25" i="0.1" d="0" iClamp="1.0" />
+ </controller>
+
+</controllers>
+''')
+
+
+calibrate_blindly('''
+<controller name="upperarm_calibration" topic="upperarm_calibration" type="JointBlindCalibrationControllerNode">
+ <calibrate joint="upperarm_roll_right_joint"
+ actuator="upperarm_roll_right_motor"
+ transmission="upperarm_roll_right_trans"
+ velocity="1.3" />
+ <pid p="5" i="0.5" d="0" iClamp="1.0" />
+</controller>
+
+''')
+
+calibrate_blindly('''
+<controller name="cal_elbow" topic="cal_elbow" type="JointBlindCalibrationControllerNode">
+ <calibrate joint="elbow_flex_right_joint"
+ actuator="elbow_flex_right_motor"
+ transmission="elbow_flex_right_trans"
+ velocity="1.0" />
+ <pid p="5" i="0.5" d="0" iClamp="1.0" />
+</controller>
+
+''')
+
+print "Calibration complete"
Property changes on: pkg/trunk/robot_descriptions/wg_robot_description/one_armed_bandit/calibrate.py
___________________________________________________________________
Added: svn:executable
+ *
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
