Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[6991] pkg/trunk
|
From: <stu...@us...> - 2008-11-19 18:06:18
|
Revision: 6991
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=6991&view=rev
Author: stuglaser
Date: 2008-11-19 18:06:10 +0000 (Wed, 19 Nov 2008)
Log Message:
-----------
New safety code (with velocity limits) and the updated robot descriptions to go with it.
Modified Paths:
--------------
pkg/trunk/mechanism/mechanism_model/include/mechanism_model/joint.h
pkg/trunk/mechanism/mechanism_model/src/joint.cpp
pkg/trunk/robot_descriptions/wg_robot_description/pr2_robot_defs/arm_defs.xml
pkg/trunk/robot_descriptions/wg_robot_description/pr2_robot_defs/base_defs.xml
pkg/trunk/robot_descriptions/wg_robot_description/pr2_robot_defs/body_defs.xml
pkg/trunk/robot_descriptions/wg_robot_description/pr2_robot_defs/head_defs.xml
Modified: pkg/trunk/mechanism/mechanism_model/include/mechanism_model/joint.h
===================================================================
--- pkg/trunk/mechanism/mechanism_model/include/mechanism_model/joint.h 2008-11-19 18:02:19 UTC (rev 6990)
+++ pkg/trunk/mechanism/mechanism_model/include/mechanism_model/joint.h 2008-11-19 18:06:10 UTC (rev 6991)
@@ -68,12 +68,12 @@
// Calibration parameters
double reference_position_; // The reading of the reference position, in radians
- /** \brief indicates wether the offset calibration value has been determined and wether the position safety parameters should be enforced.
- * \note The velocity and effort safety parameters are always enforced.
- */
- // Safety parameters
- /** Indicates if some safety limit parameters have been provided. Should be deprecated soon.*/
+
bool has_safety_limits_;
+
+ double k_position_limit_;
+ double k_velocity_limit_;
+
double spring_constant_min_;
double damping_constant_min_;
double safety_length_min_;
Modified: pkg/trunk/mechanism/mechanism_model/src/joint.cpp
===================================================================
--- pkg/trunk/mechanism/mechanism_model/src/joint.cpp 2008-11-19 18:02:19 UTC (rev 6990)
+++ pkg/trunk/mechanism/mechanism_model/src/joint.cpp 2008-11-19 18:06:10 UTC (rev 6991)
@@ -54,39 +54,47 @@
void Joint::enforceLimits(JointState *s)
{
- s->commanded_effort_ = min(max(s->commanded_effort_, -effort_limit_), effort_limit_);
+ double vel_high, vel_low;
+ double effort_high, effort_low;
- if( !(has_safety_limits_ && s->calibrated_) )
+ if (!has_safety_limits_)
return;
- //TODO: add velocity control
-
- double upper_limit = joint_limit_max_ - safety_length_max_;
- double lower_limit = joint_limit_min_ + safety_length_min_;
-
- if (s->position_ > upper_limit)
+ if(s->calibrated_ && s->joint_->type_ != JOINT_CONTINUOUS)
{
- // Damping
- if (s->velocity_ > 0)
- s->commanded_effort_ += -damping_constant_max_ * s->velocity_;
+ // Computes the position bounds based on the safety lengths.
+ double pos_high = joint_limit_max_ - safety_length_max_;
+ double pos_low = joint_limit_min_ + safety_length_min_;
- // Spring
- double offset = s->position_ - upper_limit;
- s->commanded_effort_ += -spring_constant_max_ * offset;
+ // Computes the velocity bounds based on the absolute limit and the
+ // proximity to the joint limit.
+ vel_high = min(velocity_limit_,
+ -k_position_limit_ * (s->position_ - pos_high));
+ vel_low = max(-velocity_limit_,
+ -k_position_limit_ * (s->position_ - pos_low));
}
- else if(s->position_ < lower_limit)
+ else
{
- // Damping
- if (s->velocity_ < 0)
- s->commanded_effort_ += -damping_constant_min_ * s->velocity_;
+ vel_high = velocity_limit_;
+ vel_low = -velocity_limit_;
+ }
- // Spring
- double offset = s->position_ - lower_limit;
- s->commanded_effort_ += -spring_constant_min_ * offset;
+ // Computes the effort bounds based on the velocity bounds.
+ if (velocity_limit_ >= 0.0)
+ {
+ effort_high = min(effort_limit_,
+ -k_velocity_limit_ * (s->velocity_ - vel_high));
+ effort_low = max(-effort_limit_,
+ -k_velocity_limit_ * (s->velocity_ - vel_low));
}
+ else
+ {
+ effort_high = effort_limit_;
+ effort_low = -effort_limit_;
+ }
- // One more time, just in case there are bugs in the safety limit code
- s->commanded_effort_ = min(max(s->commanded_effort_, -effort_limit_), effort_limit_);
+ s->commanded_effort_ =
+ min( max(s->commanded_effort_, effort_low), effort_high);
}
bool Joint::initXml(TiXmlElement *elt)
@@ -124,8 +132,17 @@
}
if (limits->QueryDoubleAttribute("velocity", &velocity_limit_) != TIXML_SUCCESS)
- velocity_limit_ = 0.0;
+ velocity_limit_ = -1.0;
+ else
+ {
+ if (limits->QueryDoubleAttribute("k_velocity", &k_velocity_limit_) != TIXML_SUCCESS)
+ {
+ fprintf(stderr, "No k_velocity for joint %s\n", name_.c_str());
+ return false;
+ }
+ }
+
TiXmlElement *calibration = elt->FirstChildElement("calibration");
if(calibration)
{
@@ -146,59 +163,30 @@
fprintf(stderr, "Error: no min and max limits specified for joint \"%s\"\n", name_.c_str());
return false;
}
- }
- // Safety limit code
- if (type_ == JOINT_ROTARY || type_ == JOINT_PRISMATIC)
- {
- // Loads safety limits
- TiXmlElement *safetyMinElt =elt->FirstChildElement("safety_limit_min");
-
- if(!safetyMinElt && SAFETY_LIMS_STRICTLY_ENFORCED)
- return false;
- else if (safetyMinElt)
+ if (type_ == JOINT_ROTARY || type_ == JOINT_PRISMATIC)
{
- if(safetyMinElt->QueryDoubleAttribute("spring_constant", &spring_constant_min_) != TIXML_SUCCESS)
- return false;
- if(safetyMinElt->QueryDoubleAttribute("damping_constant", &damping_constant_min_) != TIXML_SUCCESS)
- return false;
- if(safetyMinElt->QueryDoubleAttribute("safety_length", &safety_length_min_) != TIXML_SUCCESS)
- return false;
+ if (limits->QueryDoubleAttribute("k_position", &k_position_limit_) != TIXML_SUCCESS)
+ fprintf(stderr, "No k_position for joint %s\n", name_.c_str());
+ if (limits->QueryDoubleAttribute("safety_length_min", &safety_length_min_) != TIXML_SUCCESS)
+ fprintf(stderr, "No safety_length_min_ for joint %s\n", name_.c_str());
+ if (limits->QueryDoubleAttribute("safety_length_max", &safety_length_max_) != TIXML_SUCCESS)
+ fprintf(stderr, "No safety_lenght_max_ for joint %s\n", name_.c_str());
- TiXmlElement *safetyMaxElt =elt->FirstChildElement("safety_limit_max");
- if(!safetyMaxElt)
- return SAFETY_LIMS_STRICTLY_ENFORCED == false;
-
- if(safetyMaxElt->QueryDoubleAttribute("spring_constant", &spring_constant_max_) != TIXML_SUCCESS)
- return false;
- if(safetyMaxElt->QueryDoubleAttribute("damping_constant", &damping_constant_max_) != TIXML_SUCCESS)
- return false;
- if(safetyMaxElt->QueryDoubleAttribute("safety_length", &safety_length_max_) != TIXML_SUCCESS)
- return false;
-
- std::cout<<"Loaded safety limit code for joint "<<name_<<std::endl;
- std::cout<<spring_constant_min_<<" "<<damping_constant_min_<<" "<<safety_length_min_<<"\n";
-
- assert(safety_length_max_ > 0);
- assert(safety_length_min_ > 0);
- assert(joint_limit_max_ > joint_limit_min_);
- const double midpoint = 0.5*(joint_limit_max_+joint_limit_min_);
- assert(joint_limit_max_ - safety_length_max_ > midpoint);
- assert(joint_limit_min_ + safety_length_min_ < midpoint);
-
has_safety_limits_ = true;
}
}
- // Parses out the joint properties, this is done for all joints as default
+
+ // Parses out the joint properties, this is done for all joints by default
TiXmlElement *prop_el = elt->FirstChildElement("joint_properties");
if (!prop_el)
{
fprintf(stderr, "Warning: Joint \"%s\" did not specify any joint properties, default to 0.\n", name_.c_str());
joint_damping_coefficient_ = 0.0;
joint_friction_coefficient_ = 0.0;
- }
- else
+ }
+ else
{
if (prop_el->QueryDoubleAttribute("damping", &joint_damping_coefficient_) != TIXML_SUCCESS)
fprintf(stderr,"damping is not specified\n");
@@ -216,7 +204,7 @@
return false;
}
std::vector<double> axis_pieces;
- urdf::queryVectorAttribute(axis_el, "xyz", &axis_pieces);
+ urdf::queryVectorAttribute(axis_el, "xyz", &axis_pieces);
if (axis_pieces.size() != 3)
{
fprintf(stderr, "Error: The axis for joint \"%s\" must have 3 value\n", name_.c_str());
Modified: pkg/trunk/robot_descriptions/wg_robot_description/pr2_robot_defs/arm_defs.xml
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description/pr2_robot_defs/arm_defs.xml 2008-11-19 18:02:19 UTC (rev 6990)
+++ pkg/trunk/robot_descriptions/wg_robot_description/pr2_robot_defs/arm_defs.xml 2008-11-19 18:06:10 UTC (rev 6991)
@@ -4,6 +4,7 @@
xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface">
<property name="M_PI" value="3.1415926535897931" />
+ <property name="VELOCITY_LIMIT_SCALE" value="0.6" />
<property name="shoulder_pitch_length" value="0.10" />
<property name="shoulder_pitch_radius" value="0.12" />
@@ -17,10 +18,8 @@
<joint name="${prefix}_l_finger_joint" type="revolute">
<axis xyz="0 0 1" />
<anchor xyz="0 0 0" />
- <limit effort="100" velocity="1000" min="0" max="${gripper_max_angle}" />
+ <limit effort="100" min="0" max="${gripper_max_angle}" />
<calibration reference_position="0" values="0 0" />
- <safety_limit_min spring_constant="10.0" safety_length="0.1" damping_constant="0.1"/>
- <safety_limit_max spring_constant="10.0" safety_length="0.1" damping_constant="0.1"/>
<joint_properties damping="0.1" />
</joint>
@@ -62,10 +61,8 @@
<joint name="${prefix}_r_finger_joint" type="revolute">
<axis xyz="0 0 1" />
<anchor xyz="0 0 0" />
- <limit effort="100" velocity="1000" min="${-gripper_max_angle}" max="0" />
+ <limit effort="100" min="${-gripper_max_angle}" max="0" />
<calibration reference_position="0" values="0 0" />
- <safety_limit_min spring_constant="10.0" safety_length="0.1" damping_constant="0.1"/>
- <safety_limit_max spring_constant="10.0" safety_length="0.1" damping_constant="0.1"/>
<joint_properties damping="0.1" />
<mimic name="${prefix}_l_finger_joint" mult="${-1}" offset="0"/>
<map name="${prefix}_r_finger_mimic" flag="gazebo">
@@ -113,7 +110,7 @@
<joint name="${prefix}_l_finger_tip_joint" type="revolute">
<axis xyz="0 0 1" />
<anchor xyz="0 0 0" />
- <limit effort="100" velocity="1000" min="${-gripper_max_angle}" max="0" />
+ <limit effort="100" min="${-gripper_max_angle}" max="0" />
<calibration reference_position="0" values="0 0" />
<safety_limit_min spring_constant="10.0" safety_length="0.1" damping_constant="0.1"/>
<safety_limit_max spring_constant="10.0" safety_length="0.1" damping_constant="0.1"/>
@@ -164,7 +161,7 @@
<joint name="${prefix}_r_finger_tip_joint" type="revolute">
<axis xyz="0 0 1" />
<anchor xyz="0 0 0" />
- <limit effort="100" velocity="1000" min="0" max="${gripper_max_angle}" />
+ <limit effort="100" min="0" max="${gripper_max_angle}" />
<calibration reference_position="0" values="0 0" />
<safety_limit_min spring_constant="10.0" safety_length="0.1" damping_constant="0.1"/>
<safety_limit_max spring_constant="10.0" safety_length="0.1" damping_constant="0.1"/>
@@ -373,12 +370,13 @@
<joint name="${side}_shoulder_pan_joint" type="revolute">
<axis xyz="0 0 1" />
- <!-- TODO: the arm on the test stand has the optical switch at 0. It is actually
- at 45 degrees -->
- <limit min="-1.5" max="1.5" effort="30" velocity="5" />
- <calibration reference_position="0" values="1.5 -1" />
- <safety_limit_min safety_length="0.15" damping_constant="20" spring_constant="100" />
- <safety_limit_max safety_length="0.15" damping_constant="20" spring_constant="100" />
+
+ <limit min="${reflect*M_PI/4-1.5}" max="${reflect*M_PI/4+1.5}"
+ effort="30" velocity="${VELOCITY_LIMIT_SCALE*3.48}"
+ k_position="100" k_velocity="10"
+ safety_length_min="0.15" safety_length_max="0.15" />
+ <calibration reference_position="${reflect*M_PI/4}" values="1.5 -1" />
+
<joint_properties damping="10.0" />
</joint>
@@ -423,10 +421,12 @@
<joint name="${side}_shoulder_pitch_joint" type="revolute">
<axis xyz="0 1 0" />
<anchor xyz="0 0 0" />
- <limit min="-0.4" max="1.5" effort="30" velocity="5" />
+
+ <limit min="-0.4" max="1.5" effort="30" velocity="${VELOCITY_LIMIT_SCALE*3.47}"
+ k_position="100" k_velocity="10"
+ safety_length_min="0.1" safety_length_max="0.1" />
<calibration reference_position="0" values="1.5 -1 " />
- <safety_limit_min safety_length="0.1" spring_constant="50" damping_constant="12"/>
- <safety_limit_max safety_length="0.1" spring_constant="50" damping_constant="12"/>
+
<joint_properties damping="100.0" />
</joint>
@@ -471,10 +471,12 @@
<joint name="${side}_upper_arm_roll_joint" type="revolute">
<axis xyz="1 0 0" />
<anchor xyz="0 0 0" />
- <limit min="-3.9" max="0.8" effort="30" velocity="5" />
+
+ <limit min="-3.9" max="0.8" effort="30" velocity="${VELOCITY_LIMIT_SCALE*5.45}"
+ k_position="100" k_velocity="2"
+ safety_length_min="0.15" safety_length_max="0.15" />
<calibration reference_position="${-M_PI/2}" values="1.5 -1 " />
- <safety_limit_min safety_length="0.15" spring_constant="80" damping_constant="5"/>
- <safety_limit_max safety_length="0.15" spring_constant="80" damping_constant="5"/>
+
<joint_properties damping="1.0" />
</joint>
@@ -523,10 +525,12 @@
<joint name="${side}_elbow_flex_joint" type="revolute">
<axis xyz="0 -1 0" />
<anchor xyz="0 0 0" />
- <limit min="-0.1" max="2.3" effort="30" velocity="5" />
+
+ <limit min="-0.1" max="2.3" effort="30" velocity="${VELOCITY_LIMIT_SCALE*5.5}"
+ k_position="100" k_velocity="3"
+ safety_length_min="0.1" safety_length_max="0.4" />
<calibration reference_position="1.1" values="1.5 -1" />
- <safety_limit_min safety_length="0.1" spring_constant="60" damping_constant="6"/>
- <safety_limit_max safety_length="0.4" spring_constant="60" damping_constant="6"/>
+
<joint_properties damping="10.0" />
</joint>
@@ -622,8 +626,10 @@
<joint name="${side}_forearm_roll_joint" type="revolute">
<axis xyz="-1 0 0" />
<anchor xyz="0 0 0" />
- <limit effort="100" velocity="5" />
+
+ <limit effort="30" velocity="${VELOCITY_LIMIT_SCALE*6}" k_velocity="1" />
<calibration reference_position="0" values="1.5 -1" />
+
<joint_properties damping="1.0" />
</joint>
@@ -668,10 +674,12 @@
<joint name="${side}_wrist_flex_joint" type="revolute">
<axis xyz="0 -1 0" />
<anchor xyz="0 0 0" />
- <limit min="-0.1" max="2.2" effort="200" velocity="5" />
+
+ <limit min="-0.1" max="2.2" effort="200" velocity="${VELOCITY_LIMIT_SCALE*5.13}"
+ k_position="20" k_velocity="4"
+ safety_length_min="0.2" safety_length_max="0.2" />
<calibration reference_position="0.4" values="1.5 -1" />
- <safety_limit_min safety_length="0.2" spring_constant="15" damping_constant="4"/>
- <safety_limit_max safety_length="0.2" spring_constant="15" damping_constant="4"/>
+
<joint_properties damping="1.0" />
</joint>
@@ -710,8 +718,7 @@
<joint name="${side}_wrist_roll_joint" type="revolute">
<axis xyz="1 0 0" />
<anchor xyz="0 0 0" />
- <limit effort="10" velocity="5" />
- <!--<calibration reference_position="${-0.3+M_PI/2}" values="1.5 -1" />-->
+ <limit effort="10" velocity="${VELOCITY_LIMIT_SCALE*6}" k_velocity="2" />
<calibration reference_position="1.27" values="1.5 -1" />
<joint_properties damping="1.0" />
</joint>
Modified: pkg/trunk/robot_descriptions/wg_robot_description/pr2_robot_defs/base_defs.xml
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description/pr2_robot_defs/base_defs.xml 2008-11-19 18:02:19 UTC (rev 6990)
+++ pkg/trunk/robot_descriptions/wg_robot_description/pr2_robot_defs/base_defs.xml 2008-11-19 18:06:10 UTC (rev 6991)
@@ -12,11 +12,11 @@
<macro name="pr2_wheel" params="suffix parent reflect">
<joint name="wheel_${suffix}_joint" type="revolute">
- <axis xyz=" 0 1 0 " />
+ <axis xyz="0 1 0" />
<anchor xyz="0 0 0" />
- <limit effort="100" velocity="5" />
+ <limit effort="100" velocity="100" k_velocity="10" />
<calibration values="1.5 -1" />
- <joint_properties damping="0.0" friction="0.0" />
+ <joint_properties damping="1.0" friction="0.0" />
</joint>
<link name="wheel_${suffix}">
@@ -63,8 +63,8 @@
<joint name="caster_${suffix}_joint" type="revolute">
<axis xyz="0 0 1" />
<anchor xyz="0 0 0" />
+ <limit effort="100" velocity="100" k_velocity="10" />
<calibration values="1.5 -1" />
- <limit effort="100" velocity="5" />
<joint_properties damping="1.0" friction="0.0" />
</joint>
Modified: pkg/trunk/robot_descriptions/wg_robot_description/pr2_robot_defs/body_defs.xml
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description/pr2_robot_defs/body_defs.xml 2008-11-19 18:02:19 UTC (rev 6990)
+++ pkg/trunk/robot_descriptions/wg_robot_description/pr2_robot_defs/body_defs.xml 2008-11-19 18:06:10 UTC (rev 6991)
@@ -7,7 +7,10 @@
<macro name="pr2_torso" params="name parent *origin">
<joint name="${name}_joint" type="prismatic">
- <limit min="0.0" max="0.396" effort="10000" velocity="5" />
+ <limit min="0.0" max="0.396" effort="10000"
+ velocity="5" k_velocity="1000"
+ safety_length_min="0.01" safety_length_max="0.01" k_position="100" />
+ <calibration reference_position="0.00536" values="0 0" />
<axis xyz="0 0 1" />
<joint_properties damping="10.0" />
</joint>
@@ -15,9 +18,7 @@
<transmission type="SimpleTransmission" name="${name}_trans">
<actuator name="${name}_motor" />
<joint name="${name}_joint" />
- <!-- <mechanicalReduction>19.26</mechanicalReduction> -->
- <mechanicalReduction>-${52143.33}</mechanicalReduction><!-- measured by Stu and Keenan -->
- <calibration reference_position="0.00536" values="0 0" />
+ <mechanicalReduction>-52143.33</mechanicalReduction>
</transmission>
<link name="${name}">
@@ -61,7 +62,7 @@
actuator="torso_motor"
transmission="torso_trans"
velocity="-2.0" />
- <pid p="6000.0" i="10" d="0" iClamp="1000" />
+ <pid p="20000.0" i="10" d="0" iClamp="1000" />
</controller>
</macro>
Modified: pkg/trunk/robot_descriptions/wg_robot_description/pr2_robot_defs/head_defs.xml
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description/pr2_robot_defs/head_defs.xml 2008-11-19 18:02:19 UTC (rev 6990)
+++ pkg/trunk/robot_descriptions/wg_robot_description/pr2_robot_defs/head_defs.xml 2008-11-19 18:06:10 UTC (rev 6991)
@@ -11,7 +11,9 @@
<joint name="${name}_mount_joint" type="revolute">
<axis xyz="0 1 0" />
<anchor xyz="0 0 0" />
- <limit min="-0.785" max="1.48" effort="0.5292" velocity="1256.0" />
+ <limit min="-0.785" max="1.48" effort="0.5292"
+ velocity="100" k_velocity="0.05"
+ safety_length_min="0.15" safety_length_max="0.15" k_position="100" />
<calibration reference_position="-0.35" values="0 0" />
<joint_properties damping="0.1" />
</joint>
@@ -212,10 +214,10 @@
<joint name="${name}_joint" type="revolute">
<axis xyz="0 0 1" />
<limit min="-2.92" max="2.92"
- effort="2.645" velocity="1256.64" />
+ effort="2.645"
+ velocity="100" k_velocity="1.5"
+ safety_length_min="0.15" safety_length_max="0.15" k_position="100" />
<calibration reference_position="-2.79" values="0 0" />
- <safety_limit_min spring_constant="15.0" safety_length="0.15" damping_constant="0.7"/>
- <safety_limit_max spring_constant="15.0" safety_length="0.15" damping_constant="0.7"/>
<joint_properties damping="1.0" />
</joint>
@@ -262,11 +264,10 @@
<joint name="${name}_joint" type="revolute">
<axis xyz="0 1 0" />
- <limit min="-0.55" max="1.047"
- effort="12.21" velocity="1256.64" />
+ <limit min="-0.55" max="1.047" effort="12.21"
+ velocity="100" k_velocity="1.5"
+ safety_length_min="0.1" safety_length_max="0.1" k_position="100" />
<calibration reference_position="-0.195" values="0 0" />
- <safety_limit_min spring_constant="10.0" safety_length="0.1" damping_constant="0.1"/>
- <safety_limit_max spring_constant="10.0" safety_length="0.1" damping_constant="0.1"/>
<joint_properties damping="1.0" />
</joint>
@@ -341,7 +342,7 @@
actuator="head_tilt_motor"
transmission="head_tilt_trans"
velocity="-0.7" />
- <pid p="2.0" i="0.0" d="0" iClamp="5.0" />
+ <pid p="4.0" i="0.0" d="0" iClamp="5.0" />
</controller>
</macro>
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
