Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[3882] pkg/trunk
|
From: <stu...@us...> - 2008-09-02 23:46:02
|
Revision: 3882
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=3882&view=rev
Author: stuglaser
Date: 2008-09-02 23:46:04 +0000 (Tue, 02 Sep 2008)
Log Message:
-----------
The gripper transmission associates the single gripper motor with all the joints.
Modified Paths:
--------------
pkg/trunk/mechanism/mechanism_model/CMakeLists.txt
pkg/trunk/mechanism/mechanism_model/include/mechanism_model/transmission.h
pkg/trunk/robot_descriptions/wg_robot_description/pr2/transmissions.xml
Added Paths:
-----------
pkg/trunk/mechanism/mechanism_model/include/mechanism_model/gripper_transmission.h
pkg/trunk/mechanism/mechanism_model/src/gripper_transmission.cpp
Modified: pkg/trunk/mechanism/mechanism_model/CMakeLists.txt
===================================================================
--- pkg/trunk/mechanism/mechanism_model/CMakeLists.txt 2008-09-02 23:44:37 UTC (rev 3881)
+++ pkg/trunk/mechanism/mechanism_model/CMakeLists.txt 2008-09-02 23:46:04 UTC (rev 3882)
@@ -1,4 +1,4 @@
cmake_minimum_required(VERSION 2.6)
include(rosbuild)
rospack(mechanism_model)
-rospack_add_library(mechanism_model src/simple_transmission.cpp src/joint.cpp src/robot.cpp src/link.cpp src/chain.cpp src/pid.cpp)
+rospack_add_library(mechanism_model src/simple_transmission.cpp src/gripper_transmission.cpp src/joint.cpp src/robot.cpp src/link.cpp src/chain.cpp src/pid.cpp)
Added: pkg/trunk/mechanism/mechanism_model/include/mechanism_model/gripper_transmission.h
===================================================================
--- pkg/trunk/mechanism/mechanism_model/include/mechanism_model/gripper_transmission.h (rev 0)
+++ pkg/trunk/mechanism/mechanism_model/include/mechanism_model/gripper_transmission.h 2008-09-02 23:46:04 UTC (rev 3882)
@@ -0,0 +1,81 @@
+/*
+ * 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.
+ */
+
+/*
+ * <transmission type="GripperTransmission" name="gripper_l_transmission">
+ * <actuator name="gripper_l_motor" />
+ * <joint name="gripper_l_upper1_joint" reduction="4" />
+ * <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>
+ *
+ * Author: Stuart Glaser
+ */
+
+#ifndef GRIPPER_TRANSMISSION_H
+#define GRIPPER_TRANSMISSION_H
+
+#include <vector>
+#include "tinyxml/tinyxml.h"
+#include "mechanism_model/transmission.h"
+#include "mechanism_model/robot.h"
+#include "mechanism_model/pid.h"
+
+namespace mechanism {
+
+class GripperTransmission : public Transmission
+{
+public:
+ GripperTransmission() {}
+ virtual ~GripperTransmission() {}
+
+ bool initXml(TiXmlElement *config, Robot *robot);
+
+ void propagatePosition();
+ void propagatePositionBackwards();
+ void propagateEffort();
+ void propagateEffortBackwards();
+
+ Actuator *actuator_;
+ std::vector<Joint*> joints_;
+ std::vector<double> reductions_; // Mechanical reduction for each joint
+ std::vector<Pid> pids_; // For keeping the joint angles aligned in Gazebo
+
+ double motor_torque_constant_;
+ double pulses_per_revolution_;
+};
+
+} // namespace mechanism
+
+#endif
Modified: pkg/trunk/mechanism/mechanism_model/include/mechanism_model/transmission.h
===================================================================
--- pkg/trunk/mechanism/mechanism_model/include/mechanism_model/transmission.h 2008-09-02 23:44:37 UTC (rev 3881)
+++ pkg/trunk/mechanism/mechanism_model/include/mechanism_model/transmission.h 2008-09-02 23:46:04 UTC (rev 3882)
@@ -64,7 +64,7 @@
virtual bool initXml(TiXmlElement *config, Robot *robot) = 0;
// another way to initialize simple transmission
- virtual void initTransmission(std::string transmission_name,std::string joint_name,std::string actuator_name,double mechanical_reduction,double motor_torque_constant,double pulses_per_revolution, Robot *robot) = 0;
+ virtual void initTransmission(std::string transmission_name,std::string joint_name,std::string actuator_name,double mechanical_reduction,double motor_torque_constant,double pulses_per_revolution, Robot *robot) {}
// Uses encoder data to fill out joint position and velocities
virtual void propagatePosition() = 0;
Added: pkg/trunk/mechanism/mechanism_model/src/gripper_transmission.cpp
===================================================================
--- pkg/trunk/mechanism/mechanism_model/src/gripper_transmission.cpp (rev 0)
+++ pkg/trunk/mechanism/mechanism_model/src/gripper_transmission.cpp 2008-09-02 23:46:04 UTC (rev 3882)
@@ -0,0 +1,149 @@
+/*
+ * 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 "mechanism_model/gripper_transmission.h"
+#include <algorithm>
+#include <numeric>
+
+namespace mechanism {
+
+ROS_REGISTER_TRANSMISSION(GripperTransmission)
+
+bool GripperTransmission::initXml(TiXmlElement *config, Robot *robot)
+{
+ TiXmlElement *ael = config->FirstChildElement("actuator");
+ const char *actuator_name = ael ? ael->Attribute("name") : NULL;
+ actuator_ = actuator_name ? robot->getActuator(actuator_name) : NULL;
+ if (!actuator_)
+ {
+ fprintf(stderr, "GripperTransmission could not find actuator named \"%s\"\n", actuator_name);
+ return false;
+ }
+
+ for (TiXmlElement *j = config->FirstChildElement("joint"); j; j = j->NextSiblingElement("joint"))
+ {
+ const char *joint_name = j->Attribute("name");
+ Joint* joint = joint_name ? robot->getJoint(joint_name) : NULL;
+ if (!joint)
+ {
+ fprintf(stderr, "GripperTransmission could not find joint named \"%s\"\n", joint_name);
+ return false;
+ }
+ joints_.push_back(joint);
+
+ const char *joint_red = j->Attribute("reduction");
+ if (!joint_red)
+ {
+ fprintf(stderr, "GripperTransmission's joint \"%s\" was not given a reduction.\n", joint_name);
+ return false;
+ }
+ reductions_.push_back(atof(joint_red));
+ }
+
+ pids_.resize(joints_.size());
+ TiXmlElement *pel = config->FirstChildElement("pid");
+ if (pel)
+ {
+ Pid pid;
+ pid.initXml(pel);
+ for (unsigned int i = 0; i < pids_.size(); ++i)
+ pids_[i] = pid;
+ }
+
+ motor_torque_constant_ = atof(config->FirstChildElement("motorTorqueConstant")->GetText()),
+ pulses_per_revolution_ = atof(config->FirstChildElement("pulsesPerRevolution")->GetText());
+
+ return true;
+}
+
+void GripperTransmission::propagatePosition()
+{
+ assert(actuator_);
+ for (unsigned int i = 0; i < joints_.size(); ++i)
+ {
+ double mr = reductions_[i];
+ joints_[i]->position_ = actuator_->state_.encoder_count_ * 2 * M_PI / (mr * pulses_per_revolution_);
+ joints_[i]->velocity_ = actuator_->state_.encoder_velocity_ * 2 * M_PI / (mr * pulses_per_revolution_);
+ joints_[i]->applied_effort_ = actuator_->state_.last_measured_current_ * mr * motor_torque_constant_;
+ }
+}
+
+void GripperTransmission::propagatePositionBackwards()
+{
+ double mean_encoder = 0.0;
+ double mean_encoder_v = 0.0;
+ double mean_current = 0.0;
+ for (unsigned int i = 0; i < joints_.size(); ++i)
+ {
+ double mr = reductions_[i];
+ mean_encoder += joints_[i]->position_ * mr * pulses_per_revolution_ / (2 * M_PI);
+ mean_encoder_v += joints_[i]->velocity_ * mr * pulses_per_revolution_ / (2 * M_PI);
+ mean_current += joints_[i]->applied_effort_ / (mr * motor_torque_constant_);
+ }
+ actuator_->state_.encoder_count_ = mean_encoder / joints_.size();
+ actuator_->state_.encoder_velocity_ = mean_encoder_v / joints_.size();
+ actuator_->state_.last_measured_current_ = mean_current / joints_.size();
+}
+
+void GripperTransmission::propagateEffort()
+{
+ double strongest = 0.0;
+ for (unsigned int i = 0; i < joints_.size(); ++i)
+ {
+ if (fabs(joints_[i]->commanded_effort_ / (reductions_[i])) > fabs(strongest))
+ strongest = joints_[i]->commanded_effort_ / (reductions_[i] * motor_torque_constant_);
+ }
+ actuator_->command_.current_ = strongest;
+}
+
+void GripperTransmission::propagateEffortBackwards()
+{
+ std::vector<double> scaled_positions(joints_.size());
+ for (unsigned int i = 0; i < joints_.size(); ++i)
+ scaled_positions[i] = joints_[i]->position_ * reductions_[i];
+
+ double mean = std::accumulate(scaled_positions.begin(), scaled_positions.end(), 0.0)
+ / scaled_positions.size();
+
+ for (unsigned int i = 0; i < joints_.size(); ++i)
+ {
+ double err = scaled_positions[i] - mean;
+ double pid_effort = pids_[i].updatePid(err, 0.001);
+
+ joints_[i]->commanded_effort_ =
+ pid_effort / reductions_[i] +
+ actuator_->command_.current_ * reductions_[i] * motor_torque_constant_;
+ }
+}
+
+} // namespace mechanism
Modified: pkg/trunk/robot_descriptions/wg_robot_description/pr2/transmissions.xml
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description/pr2/transmissions.xml 2008-09-02 23:44:37 UTC (rev 3881)
+++ pkg/trunk/robot_descriptions/wg_robot_description/pr2/transmissions.xml 2008-09-02 23:46:04 UTC (rev 3882)
@@ -151,10 +151,13 @@
</transmission>
<!-- fingers share a single motor -->
- <transmission type="SimpleTransmission" name="gripper_left_trans">
+ <transmission type="GripperTransmission" name="gripper_left_trans">
<actuator name="gripper_left_motor" />
- <joint name="gripper_left_joint" />
- <mechanicalReduction>1</mechanicalReduction>
+ <joint name="finger_r_left_joint" reduction="-1.0" />
+ <joint name="finger_tip_r_left_joint" reduction="1.0" />
+ <joint name="finger_l_left_joint" reduction="1.0" />
+ <joint name="finger_tip_l_left_joint" reduction="-1.0" />
+ <pid p="0.10" i="0.0" d="0.005" iClamp="0.5" />
<motorTorqueConstant>1</motorTorqueConstant>
<pulsesPerRevolution>90000</pulsesPerRevolution>
</transmission>
@@ -218,10 +221,13 @@
</transmission>
<!-- fingers share a single motor -->
- <transmission type="SimpleTransmission" name="gripper_right_trans">
+ <transmission type="GripperTransmission" name="gripper_right_trans">
<actuator name="gripper_right_motor" />
- <joint name="gripper_right_joint" />
- <mechanicalReduction>1</mechanicalReduction>
+ <joint name="finger_r_right_joint" reduction="-1.0" />
+ <joint name="finger_tip_r_right_joint" reduction="1.0" />
+ <joint name="finger_l_right_joint" reduction="1.0" />
+ <joint name="finger_tip_l_right_joint" reduction="-1.0" />
+ <pid p="0.10" i="0.0" d="0.005" iClamp="0.5" />
<motorTorqueConstant>1</motorTorqueConstant>
<pulsesPerRevolution>90000</pulsesPerRevolution>
</transmission>
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
