Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[4601] pkg/trunk
|
From: <stu...@us...> - 2008-09-23 20:03:25
|
Revision: 4601
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=4601&view=rev
Author: stuglaser
Date: 2008-09-23 20:03:09 +0000 (Tue, 23 Sep 2008)
Log Message:
-----------
Calibration back up and running.
Modified Paths:
--------------
pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_blind_calibration_controller.h
pkg/trunk/controllers/robot_mechanism_controllers/src/joint_blind_calibration_controller.cpp
pkg/trunk/mechanism/mechanism_control/src/mechanism_control/mechanism.py
pkg/trunk/mechanism/mechanism_control/src/mechanism_control.cpp
pkg/trunk/mechanism/mechanism_control/srv/SpawnController.srv
Added Paths:
-----------
pkg/trunk/manip/teleop_robot/scripts/calibrate.py
Modified: pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_blind_calibration_controller.h
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_blind_calibration_controller.h 2008-09-23 20:02:30 UTC (rev 4600)
+++ pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_blind_calibration_controller.h 2008-09-23 20:03:09 UTC (rev 4601)
@@ -43,6 +43,15 @@
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.
+ Example XML:
+ <controller type="JointBlindCalibrationController">
+ <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>
+
*/
/***************************************************/
@@ -56,7 +65,7 @@
namespace controller
{
-class JointBlindCalibrationController : public JointManualCalibrationController
+class JointBlindCalibrationController : public Controller
{
public:
/*!
@@ -82,17 +91,26 @@
*/
virtual void update();
+ bool calibrated() { return state_ == DONE; }
+ void beginCalibration() {
+ if (state_ == INITIALIZED)
+ state_ = BEGINNING;
+ }
+
protected:
- enum {SearchUp=100,SearchDown,SearchingUp,SearchingDown};
+ enum { INITIALIZED, BEGINNING, STARTING_UP, MOVING_UP,
+ STARTING_DOWN, MOVING_DOWN, DONE };
+ int state_;
double search_velocity_;
+ Actuator *actuator_;
+ mechanism::JointState *joint_;
+ mechanism::Transmission *transmission_;
- double velocity_cmd_;
-
double init_time;
- controller::JointVelocityController vcontroller_; /** The joint velocity controller used to sweep the joint.*/
+ controller::JointVelocityController vc_; /** The joint velocity controller used to sweep the joint.*/
};
Modified: pkg/trunk/controllers/robot_mechanism_controllers/src/joint_blind_calibration_controller.cpp
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/src/joint_blind_calibration_controller.cpp 2008-09-23 20:02:30 UTC (rev 4600)
+++ pkg/trunk/controllers/robot_mechanism_controllers/src/joint_blind_calibration_controller.cpp 2008-09-23 20:03:09 UTC (rev 4601)
@@ -41,145 +41,144 @@
ROS_REGISTER_CONTROLLER(JointBlindCalibrationController)
JointBlindCalibrationController::JointBlindCalibrationController()
- : JointManualCalibrationController()
+: state_(INITIALIZED), joint_(NULL)
{
- std::cout<<"JointBlindCalibration created\n";
}
JointBlindCalibrationController::~JointBlindCalibrationController()
{
- std::cout<<"JointBlindCalibration destroyed\n";
}
-
+#define GOTHERE printf("HERE %s %d\n", __FILE__, __LINE__);
bool JointBlindCalibrationController::initXml(mechanism::RobotState *robot, TiXmlElement *config)
{
assert(robot);
assert(config);
- robot_ = robot->model_;
- bool base=JointManualCalibrationController::initXml(robot, config);
- if(!base)
- return false;
- TiXmlElement *j = config->FirstChildElement("param");
- if (!j)
+
+ TiXmlElement *cal = config->FirstChildElement("calibrate");
+ if (!cal)
{
- std::cerr<<"JointBlindCalibrationController was not given parameters"<<std::endl;
+ std::cerr<<"JointBlindCalibrationController was not given calibration parameters"<<std::endl;
return false;
}
- if(j->QueryDoubleAttribute("velocity", &search_velocity_ ) != TIXML_SUCCESS)
+ if(cal->QueryDoubleAttribute("velocity", &search_velocity_) != TIXML_SUCCESS)
{
std::cerr<<"Velocity value was not specified\n";
return false;
}
- std::cout<<"search vel"<<search_velocity_<<std::endl;
- if(search_velocity_ == 0)
+
+ const char *joint_name = cal->Attribute("joint");
+ joint_ = joint_name ? robot->getJointState(joint_name) : NULL;
+ if (!joint_)
{
- std::cerr<<"You gave zero velocity\n";
+ fprintf(stderr, "Error: JointBlindCalibrationController could not find joint \"%s\"\n",
+ joint_name);
return false;
}
- TiXmlElement *v = config->FirstChildElement("controller");
- if(!v)
+ const char *actuator_name = cal->Attribute("actuator");
+ actuator_ = actuator_name ? robot->model_->getActuator(actuator_name) : NULL;
+ if (!actuator_)
{
- std::cerr<<"JointBlindCalibrationController was not given a controller to move the joint."<<std::endl;
+ fprintf(stderr, "Error: JointBlindCalibrationController could not find actuator \"%s\"\n",
+ actuator_name);
return false;
}
- return vcontroller_.initXml(robot, v);
+ const char *transmission_name = cal->Attribute("transmission");
+ transmission_ = transmission_name ? robot->model_->getTransmission(transmission_name) : NULL;
+ if (!transmission_)
+ {
+ fprintf(stderr, "Error: JointBlindCalibrationController 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: JointBlindCalibrationController 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 JointBlindCalibrationController::update()
{
- static const double v_thresh = 0.3*std::abs(search_velocity_);
- assert(joint_state_);
+ assert(joint_);
assert(actuator_);
- if(state_ == Begin)
- {
- std::cout<<"begin procedure"<<std::endl;
- joint_state_->calibrated_ = false;
- init_time= robot_->hw_->current_time_;
- velocity_cmd_ = search_velocity_;
- if(search_velocity_>0)
- state_ = SearchUp;
- else
- state_ = SearchDown;
- }
+ static int count = 0;
+ static double joint_max_raw, joint_min_raw; // Joint angles at the bumps
- if(state_ == SearchUp)
+ switch (state_)
{
- if(joint_state_->velocity_ > v_thresh)
+ case INITIALIZED:
+ return;
+ case BEGINNING:
+ state_ = STARTING_UP;
+ count = 0;
+ joint_->calibrated_ = false;
+ actuator_->state_.zero_offset_ = 0.0;
+ break;
+ case STARTING_UP:
+ vc_.setCommand(search_velocity_);
+ if (++count > 500)
{
- std::cout<<"Searching up\n";
- state_ = SearchingUp;
+ count = 0;
+ state_ = MOVING_UP;
}
- else
- std::cout<<joint_state_->velocity_ <<'\t'<< v_thresh <<std::endl;
- }
-
- if(state_ == SearchDown)
- {
- if(joint_state_->velocity_ < -v_thresh)
+ break;
+ case MOVING_UP:
+ if (fabs(joint_->velocity_) < 0.001)
{
- std::cout<<"Searching down\n";
- state_ = SearchingDown;
+ joint_max_raw = joint_->position_;
+ state_ = STARTING_DOWN;
+ count = 0;
}
+ break;
+ case STARTING_DOWN:
+ vc_.setCommand(-search_velocity_);
+ if (++count > 500)
+ {
+ state_ = MOVING_DOWN;
+ count = 0;
+ }
+ break;
+ case MOVING_DOWN:
+ if (fabs(joint_->velocity_) < 0.001)
+ {
+ joint_min_raw = joint_->position_;
- }
+ // Sets the desired joint zero based on where we bumped the limits.
+ double joint_zero = ((joint_max_raw - joint_->joint_->joint_limit_max_) +
+ (joint_min_raw - joint_->joint_->joint_limit_min_)) / 2.0;
+ std::vector<Actuator*> fake_a;
+ std::vector<mechanism::JointState*> fake_j;
+ fake_a.push_back(new Actuator);
+ fake_j.push_back(new mechanism::JointState);
+ fake_j[0]->position_ = joint_zero;
+ transmission_->propagatePositionBackwards(fake_j, fake_a);
+ actuator_->state_.zero_offset_ = fake_a[0]->state_.position_;
+ joint_->calibrated_ = true;
-// const double cur_time = robot_->hw_->current_time_;
-//
-// if(cur_time - init_time > 1000)
-// {
-// std::cout<<"Finished\n";
-// const double offset_ = offset(actuator_->state_.position_, joint_->joint_limit_max_);
-// std::cout<<"offset "<<offset_<<std::endl;
-// actuator_->state_.zero_offset_ = offset_;
-// state_ = SearchDown; //TODO: get better results by a search down
-// state_ = Initialized;
-//
-// }
-
- if(state_ == SearchingDown && joint_state_->velocity_>=0)
- {
- std::cout<<"Bump\n";
- const double offset_down = offset(actuator_->state_.position_, joint_->joint_limit_min_);
- std::cout<<"offset "<<offset_down<<std::endl;
- actuator_->state_.zero_offset_ = offset_down;
-// state_ = SearchUp; //TODO: get better results by a search down
- state_ = Initialized;
- }
-
- if(state_ == SearchingUp && joint_state_->velocity_<=0)
- {
- std::cout<<"Bump\n";
- const double offset_up = offset(actuator_->state_.position_, joint_->joint_limit_max_);
- std::cout<<"offset "<<offset_up<<std::endl;
- actuator_->state_.zero_offset_ = offset_up;
-// state_ = SearchDown; //TODO: get better results by a search down
- state_ = Initialized;
- }
-
- if(state_ == Initialized)
- {
- joint_state_->calibrated_ = true;
- velocity_cmd_ = 0;
- state_ = Stop;
- state_mutex_.unlock();
+ state_ = DONE;
+ count = 0;
+ }
+ break;
+ case DONE:
+ vc_.setCommand(0.0);
+ break;
}
- if(state_ == Stop)
- {
- velocity_cmd_ = 0;
- }
-
-
-
- if(state_!=Stop)
- {
- vcontroller_.setCommand(velocity_cmd_);
- vcontroller_.update();
- }
+ vc_.update();
}
@@ -204,10 +203,10 @@
bool JointBlindCalibrationControllerNode::calibrateCommand(robot_mechanism_controllers::CalibrateJoint::request &req, robot_mechanism_controllers::CalibrateJoint::response &resp)
{
c_->beginCalibration();
- ros::Duration d=ros::Duration(0,1000000);
+ ros::Duration d = ros::Duration(0,1000000);
while(!c_->calibrated())
d.sleep();
- c_->getOffset(resp.offset);
+ resp.offset = 0.0;
return true;
}
@@ -224,6 +223,7 @@
if (!c_->initXml(robot, config))
return false;
+
node->advertise_service(topic + "/calibrate", &JointBlindCalibrationControllerNode::calibrateCommand, this);
return true;
}
Added: pkg/trunk/manip/teleop_robot/scripts/calibrate.py
===================================================================
--- pkg/trunk/manip/teleop_robot/scripts/calibrate.py (rev 0)
+++ pkg/trunk/manip/teleop_robot/scripts/calibrate.py 2008-09-23 20:03:09 UTC (rev 4601)
@@ -0,0 +1,119 @@
+#!/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.
+#
+
+# Written by Timothy Hunter <tjh...@wi...> 2008
+
+import rostools
+import copy
+
+# Loads interface with the robot.
+rostools.update_path('teleop_robot')
+import rospy
+from mechanism_control.srv import *
+from robot_mechanism_controllers.srv import *
+from teleop_robot 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"
+
+calibrate_optically(slurp('pr2_arm/calibration_shoulder_pan.xml'))
+calibrate_optically(slurp('pr2_arm/calibration_shoulder_lift.xml'))
+
+calibrate_blindly('''
+<controller name="upperarm_calibration" topic="upperarm_calibration" type="JointBlindCalibrationControllerNode">
+ <calibrate joint="upperarm_roll_right_joint"
+ actuator="upperarm_roll_right_act"
+ transmission="upperarm_roll_right_trans"
+ velocity="0.6" />
+ <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_right_joint"
+ actuator="elbow_right_act"
+ transmission="elbow_right_trans"
+ velocity="0.6" />
+ <pid p="5" i="0.5" d="0" iClamp="1.0" />
+</controller>
+
+''')
Property changes on: pkg/trunk/manip/teleop_robot/scripts/calibrate.py
___________________________________________________________________
Added: svn:executable
+ *
Modified: pkg/trunk/mechanism/mechanism_control/src/mechanism_control/mechanism.py
===================================================================
--- pkg/trunk/mechanism/mechanism_control/src/mechanism_control/mechanism.py 2008-09-23 20:02:30 UTC (rev 4600)
+++ pkg/trunk/mechanism/mechanism_control/src/mechanism_control/mechanism.py 2008-09-23 20:03:09 UTC (rev 4601)
@@ -26,10 +26,11 @@
rospy.wait_for_service('spawn_controller')
s = rospy.ServiceProxy('spawn_controller', SpawnController)
resp = s.call(SpawnControllerRequest(xml))
- if resp.ok == 1:
- print "Spawned successfully"
- else:
- print "Error when spawning", resp.ok
+ for i in range(len(resp.ok)):
+ if resp.ok[i]:
+ print "Spawned", resp.name[i]
+ else:
+ print "Error when spawning", resp.name[i]
def kill_controller(name):
rospy.wait_for_service('kill_controller')
Modified: pkg/trunk/mechanism/mechanism_control/src/mechanism_control.cpp
===================================================================
--- pkg/trunk/mechanism/mechanism_control/src/mechanism_control.cpp 2008-09-23 20:02:30 UTC (rev 4600)
+++ pkg/trunk/mechanism/mechanism_control/src/mechanism_control.cpp 2008-09-23 20:03:09 UTC (rev 4601)
@@ -320,22 +320,41 @@
TiXmlDocument doc;
doc.Parse(req.xml_config.c_str());
+ std::vector<uint8_t> oks;
+ std::vector<std::string> names;
+
TiXmlElement *config = doc.RootElement();
- if (0 == strcmp(config->Value(), "controllers"))
+ if (!config)
+ return false;
+ if (config->ValueStr() != "controllers" &&
+ config->ValueStr() != "controller")
+ return false;
+
+ if (config->ValueStr() == "controllers")
{
config = config->FirstChildElement("controller");
}
for (; config; config = config->NextSiblingElement("controller"))
{
+ bool ok = true;
+
if (!config->Attribute("type"))
- resp.ok = false;
+ ok = false;
else if (!config->Attribute("name"))
- resp.ok = false;
+ ok = false;
else
- resp.ok = mc_->spawnController(config->Attribute("type"), config->Attribute("name"), config);
+ {
+ ok = mc_->spawnController(config->Attribute("type"), config->Attribute("name"), config);
+ }
+
+ oks.push_back(ok);
+ names.push_back(config->Attribute("name") ? config->Attribute("name") : "");
}
+ resp.set_ok_vec(oks);
+ resp.set_name_vec(names);
+
return true;
}
Modified: pkg/trunk/mechanism/mechanism_control/srv/SpawnController.srv
===================================================================
--- pkg/trunk/mechanism/mechanism_control/srv/SpawnController.srv 2008-09-23 20:02:30 UTC (rev 4600)
+++ pkg/trunk/mechanism/mechanism_control/srv/SpawnController.srv 2008-09-23 20:03:09 UTC (rev 4601)
@@ -1,3 +1,4 @@
string xml_config
---
-byte ok
+byte[] ok
+string[] name
\ No newline at end of file
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
