Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[13982] pkg/trunk
|
From: <hsu...@us...> - 2009-04-16 22:32:20
|
Revision: 13982
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=13982&view=rev
Author: hsujohnhsu
Date: 2009-04-16 22:32:09 +0000 (Thu, 16 Apr 2009)
Log Message:
-----------
- bug fix: update gazebo inertia matrix setup, dMassTranslate not needed.
- update dual link example.
- update multi link example.
- update gripper, added fake inertia scale so the large k_velocity/k_position will stablize in sim.
- update urdf2gazebo, joint limit enforced from [-PI,PI]. previously [-.9*PI,.9*PI].
Modified Paths:
--------------
pkg/trunk/3rdparty/gazebo/gazebo_patch.diff
pkg/trunk/demos/examples_gazebo/dual_link_defs/dual_link.xml
pkg/trunk/demos/examples_gazebo/multi_link_defs/multi_link.xml
pkg/trunk/drivers/simulator/gazebo_plugin/src/urdf2gazebo.cpp
pkg/trunk/robot_descriptions/pr2/pr2_defs/defs/gripper_defs.xml
Modified: pkg/trunk/3rdparty/gazebo/gazebo_patch.diff
===================================================================
--- pkg/trunk/3rdparty/gazebo/gazebo_patch.diff 2009-04-16 22:27:12 UTC (rev 13981)
+++ pkg/trunk/3rdparty/gazebo/gazebo_patch.diff 2009-04-16 22:32:09 UTC (rev 13982)
@@ -262,7 +262,40 @@
===================================================================
--- server/physics/Body.cc (revision 7599)
+++ server/physics/Body.cc (working copy)
-@@ -812,7 +812,6 @@
+@@ -809,15 +809,42 @@
+ this->comPose.pos.y = this->cy;
+ this->comPose.pos.z = this->cz;
+
++#if 0
++ // DEBUG: TESTING DYNAMICS, THIS SECTION IS NOT NEEDED
++ // apply parallel axis theorem, Huygens-Steiner Theorem, move CG to body ref frame
++ double ixx1,iyy1,izz1,ixy1,ixz1,iyz1;
++ double cx2 = pow(cx,2);
++ double cy2 = pow(cy,2);
++ double cz2 = pow(cz,2);
++ ixx1 = ixx + this->bodyMass*(cy2+cz2);
++ iyy1 = iyy + this->bodyMass*(cx2+cz2);
++ izz1 = izz + this->bodyMass*(cx2+cy2);
++ ixy1 = ixy - this->bodyMass*(cx*cy);
++ ixz1 = ixz - this->bodyMass*(cx*cz);
++ iyz1 = iyz - this->bodyMass*(cy*cz);
++
++ this->ixx = ixx1;
++ this->iyy = iyy1;
++ this->izz = izz1;
++ this->ixy = ixy1;
++ this->ixz = ixz1;
++ this->iyz = iyz1;
++ this->cx = this->cx - this->cx;
++ this->cy = this->cy - this->cy;
++ this->cz = this->cz - this->cz;
++#endif
++
++ // apply same hack as before, move cg to 0,0,0, taken care of by comPose.
++ this->cx = 0.0;
++ this->cy = 0.0;
++ this->cz = 0.0;
++
// setup this->mass as well
dMassSetParameters(&this->mass, this->bodyMass,
this->cx, this->cy, this->cz,
@@ -270,7 +303,12 @@
this->ixx,this->iyy,this->izz,
this->ixy,this->ixz,this->iyz);
-@@ -825,19 +824,27 @@
+- dMassTranslate( &this->mass, -this->cx, -this->cy, -this->cz);
+-
+ // dMatrix3 rot;
+ // dMassRotate(&this->mass, rot);
+
+@@ -825,19 +852,32 @@
if (this->mass.mass > 0)
dBodySetMass( this->bodyId, &this->mass );
@@ -286,6 +324,7 @@
- // std::cout << " I[6] " << this->mass.I[6] << std::endl;
- // std::cout << " I[7] " << this->mass.I[7] << std::endl;
- // std::cout << " I[8] " << this->mass.I[8] << std::endl;
++#define DEBUG
+#ifdef DEBUG
+#define _I(i,j) I[(i)*4+(j)]
+ std::cout << " name: " << this->GetName() << std::endl;
@@ -304,7 +343,11 @@
+ std::cout << " I(2,2) " << this->mass._I(2,2) << std::endl;
+ // check and make sure no inertias less than zero. if less than zero, the inertia matrix + mass + cg combo is a bad one.
-+ assert(this->mass._I(0,0)>0.0 && this->mass._I(1,1)>0.0 && this->mass._I(2,2)>0.0);
++ if (this->mass._I(0,0) < 0.0 || this->mass._I(1,1) < 0.0 || this->mass._I(2,2) < 0.0)
++ {
++ //assert(0);
++ std::cout << "ERROR: ixx,iyy or izz < 0" << std::endl;
++ }
+#endif
+
}
Modified: pkg/trunk/demos/examples_gazebo/dual_link_defs/dual_link.xml
===================================================================
--- pkg/trunk/demos/examples_gazebo/dual_link_defs/dual_link.xml 2009-04-16 22:27:12 UTC (rev 13981)
+++ pkg/trunk/demos/examples_gazebo/dual_link_defs/dual_link.xml 2009-04-16 22:32:09 UTC (rev 13982)
@@ -21,21 +21,19 @@
<axis xyz="0 0 1" />
<anchor xyz="0 0 0" />
<limit min="0.0" max="0.0" effort="100" velocity="5"
- k_position="1.0" k_velocity="1.0"
+ k_position="0.0" k_velocity="0.0"
safety_length_min="0.0" safety_length_max="0.0" />
<calibration values="1.5 -1 " />
- <joint_properties damping="10.0" />
+ <joint_properties damping="0.0" />
</joint>
<!-- joint blocks -->
<joint name="link2_joint" type="revolute" >
<axis xyz="0 1 0" />
- <anchor xyz="0.0 0 0" />
- <limit min="-3.0" max="3.0" effort="100" velocity="5"
- k_position="1.0" k_velocity="1.0"
- safety_length_min="0.0" safety_length_max="0.0" />
+ <anchor xyz="0 0 0" />
+ <limit effort="100" velocity="5" k_velocity="1000" />
<calibration values="1.5 -1 " />
- <joint_properties damping="0.0" />
+ <joint_properties damping="0.0" friction="0.0" />
</joint>
@@ -102,9 +100,9 @@
<origin xyz="0.0 0 0" rpy="0 0 0" />
<joint name="link2_joint" />
<inertial >
- <mass value="1.0" />
- <com xyz="0.1 0 0" />
- <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0" />
+ <mass value="10.0" />
+ <com xyz="0.5 0 0" />
+ <inertia ixx="0.01" ixy="0.0" ixz="0.0" iyy="10.0" iyz="0.0" izz="0.01" />
</inertial>
<visual >
<origin xyz="0.5 0 0" rpy="0 0 0" />
Modified: pkg/trunk/demos/examples_gazebo/multi_link_defs/multi_link.xml
===================================================================
--- pkg/trunk/demos/examples_gazebo/multi_link_defs/multi_link.xml 2009-04-16 22:27:12 UTC (rev 13981)
+++ pkg/trunk/demos/examples_gazebo/multi_link_defs/multi_link.xml 2009-04-16 22:32:09 UTC (rev 13982)
@@ -21,77 +21,63 @@
<joint name="link1_joint" type="revolute" >
<axis xyz="0 0 1" />
<anchor xyz="0 0 0" />
- <safety_limit_min spring_constant="0.0" safety_length="0.1" damping_constant="0.0" />
- <safety_limit_max spring_constant="0.0" safety_length="0.1" damping_constant="0.0" />
- <limit k_position="1.0" k_velocity="1.0" min="-.5" max="0.5" effort="10" velocity="50" />
+ <limit safety_length_min="0" safety_length_max="0" k_position="1.0" k_velocity="1.0" min="-.5" max="0.5" effort="10" velocity="50" />
<calibration values="1.5 -1 " />
- <joint_properties damping="200.0" />
+ <joint_properties damping="1.0" />
</joint>
<!-- joint blocks -->
<joint name="link2_joint" type="revolute" >
<axis xyz="0 1 0" />
<anchor xyz="0.0 0 0" />
- <safety_limit_min spring_constant="0.0" safety_length="0.01" damping_constant="0.0" />
- <safety_limit_max spring_constant="0.0" safety_length="0.01" damping_constant="0.0" />
- <limit k_position="100.0" k_velocity="100.0" min="-1" max="2" effort="5000" velocity="5" />
+ <limit safety_length_min="0" safety_length_max="0" k_position="1.0" k_velocity="1.0" min="-1" max="2" effort="5000" velocity="5" />
<calibration values="1.5 -1 " />
- <joint_properties damping="100.0" />
+ <joint_properties damping="1.0" />
</joint>
<!-- joint blocks -->
<joint name="link3_joint" type="revolute" >
<axis xyz="0 1 0" />
<anchor xyz="0.0 0 0" />
- <safety_limit_min spring_constant="0.0" safety_length="0.01" damping_constant="0.0" />
- <safety_limit_max spring_constant="0.0" safety_length="0.01" damping_constant="0.0" />
- <limit k_position="100.0" k_velocity="100.0" min="-1" max="2" effort="5000" velocity="5" />
+ <limit safety_length_min="0" safety_length_max="0" k_position="1.0" k_velocity="1.0" min="-1" max="2" effort="5000" velocity="5" />
<calibration values="1.5 -1 " />
- <joint_properties damping="100.0" />
+ <joint_properties damping="1.0" />
</joint>
<!-- joint blocks -->
<joint name="link4_joint" type="revolute" >
<axis xyz="0 1 0" />
<anchor xyz="0.0 0 0" />
- <safety_limit_min spring_constant="0.0" safety_length="0.01" damping_constant="0.0" />
- <safety_limit_max spring_constant="0.0" safety_length="0.01" damping_constant="0.0" />
- <limit k_position="100.0" k_velocity="100.0" min="-1" max="2" effort="2000" velocity="5" />
+ <limit safety_length_min="0" safety_length_max="0" k_position="1.0" k_velocity="1.0" min="-1" max="2" effort="2000" velocity="5" />
<calibration values="1.5 -1 " />
- <joint_properties damping="10.0" />
+ <joint_properties damping="1.0" />
</joint>
<!-- joint blocks -->
<joint name="link5_joint" type="revolute" >
<axis xyz="0 1 0" />
<anchor xyz="0.0 0 0" />
- <safety_limit_min spring_constant="0.0" safety_length="0.01" damping_constant="0.0" />
- <safety_limit_max spring_constant="0.0" safety_length="0.01" damping_constant="0.0" />
- <limit k_position="100.0" k_velocity="100.0" min="-1" max="2" effort="500" velocity="5" />
+ <limit safety_length_min="0" safety_length_max="0" k_position="1.0" k_velocity="1.0" min="-1" max="2" effort="500" velocity="5" />
<calibration values="1.5 -1 " />
- <joint_properties damping="10.0" />
+ <joint_properties damping="1.0" />
</joint>
<!-- joint blocks -->
<joint name="link6_joint" type="revolute" >
<axis xyz="0 1 0" />
<anchor xyz="0.0 0 0" />
- <safety_limit_min spring_constant="0.0" safety_length="0.01" damping_constant="0.0" />
- <safety_limit_max spring_constant="0.0" safety_length="0.01" damping_constant="0.0" />
- <limit k_position="100.0" k_velocity="100.0" min="-1" max="2" effort="100" velocity="5" />
+ <limit safety_length_min="0" safety_length_max="0" k_position="1.0" k_velocity="1.0" min="-1" max="2" effort="100" velocity="5" />
<calibration values="1.5 -1 " />
- <joint_properties damping="10.0" />
+ <joint_properties damping="1.0" />
</joint>
<!-- joint blocks -->
<joint name="link7_joint" type="revolute" >
<axis xyz="0 1 0" />
<anchor xyz="0.0 0 0" />
- <safety_limit_min spring_constant="0.0" safety_length="0.01" damping_constant="0.0" />
- <safety_limit_max spring_constant="0.0" safety_length="0.01" damping_constant="0.0" />
- <limit k_position="100.0" k_velocity="100.0" min="-1" max="2" effort="100" velocity="5" />
+ <limit safety_length_min="0" safety_length_max="0" k_position="1.0" k_velocity="1.0" min="-1" max="2" effort="100" velocity="5" />
<calibration values="1.5 -1 " />
- <joint_properties damping="10.0" />
+ <joint_properties damping="1.0" />
</joint>
<!-- link blocks -->
@@ -126,9 +112,9 @@
<origin xyz="0 0 10" rpy="0 0 0" />
<joint name="link1_joint" />
<inertial >
- <mass value="1" />
+ <mass value="100.0" />
<com xyz="0 0 -0.5" />
- <inertia ixx="2.00" ixy="0.0" ixz="0.0" iyy="2.0" iyz="0.0" izz="2.0" />
+ <inertia ixx="200.00" ixy="0.0" ixz="0.0" iyy="200.0" iyz="0.0" izz="100.0" />
</inertial>
<visual >
<origin xyz="0 0 -0.5" rpy="0 0 0" />
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/src/urdf2gazebo.cpp
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/src/urdf2gazebo.cpp 2009-04-16 22:27:12 UTC (rev 13981)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/src/urdf2gazebo.cpp 2009-04-16 22:32:09 UTC (rev 13982)
@@ -369,20 +369,20 @@
ROS_WARN("urdf2gazebo: limiting lowStop to <= 0 degrees");
*lowstop = 0.0;
}
- if (*lowstop < -(M_PI*0.9))
+ if (*lowstop < -(M_PI))
{
- ROS_WARN("urdf2gazebo: limiting lowStop to >= -(180*0.9) degrees");
- *lowstop = -(M_PI*0.9);
+ ROS_WARN("urdf2gazebo: limiting lowStop to >= -(180) degrees");
+ *lowstop = -(M_PI);
}
if (*highstop < 0)
{
ROS_WARN("urdf2gazebo: limiting highStop to >= 0 degrees");
*highstop = 0.0;
}
- if (*highstop > (M_PI*0.9))
+ if (*highstop > (M_PI))
{
- ROS_WARN("urdf2gazebo: limiting highStop to <= (180*0.9) degrees");
- *highstop = (M_PI*0.9);
+ ROS_WARN("urdf2gazebo: limiting highStop to <= (180) degrees");
+ *highstop = (M_PI);
}
addKeyValue(joint, "lowStop", values2str(1, link->joint->limit , rad2deg));
addKeyValue(joint, "highStop", values2str(1, link->joint->limit + 1, rad2deg));
Modified: pkg/trunk/robot_descriptions/pr2/pr2_defs/defs/gripper_defs.xml
===================================================================
--- pkg/trunk/robot_descriptions/pr2/pr2_defs/defs/gripper_defs.xml 2009-04-16 22:27:12 UTC (rev 13981)
+++ pkg/trunk/robot_descriptions/pr2/pr2_defs/defs/gripper_defs.xml 2009-04-16 22:32:09 UTC (rev 13982)
@@ -4,6 +4,7 @@
xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface">
<property name="M_PI" value="3.1415926535897931" />
+ <property name="scale" value="10.0" />
<property name="gripper_max_angle" value="0.548" /> <!-- max gap size /2 -->
<property name="gripper_min_angle" value="0.00" />
@@ -12,7 +13,7 @@
<property name="finger_stop_kp" value="100000.0" />
<property name="finger_fudge_factor" value="1.0" />
- <property name="finger_damping" value="3.0" />
+ <property name="finger_damping" value="0.001" />
<property name="finger_tip_damping" value="0.001" />
<property name="finger_kp" value="0.0" />
@@ -32,7 +33,7 @@
<!-- IMPORTANT: k_velocity and k_position of 1 will induce instability on the acutal joint -->
<limit effort="${finger_joint_effort_limit}" velocity="0.5"
min="${gripper_min_angle}" max="${gripper_max_angle}"
- k_velocity="0.5" k_position="0.5"
+ k_velocity="1.0" k_position="1.0"
safety_length_min="0.0" safety_length_max="0.01" />
</macro>
@@ -65,12 +66,12 @@
<inertial>
<mass value="0.17126" />
<com xyz="${0.03598} 0.01730 -0.00164" />
- <inertia ixx= "0.00007756198"
- ixy= "0.00000149095"
- ixz="-0.00000983385"
- iyy= "0.00019708305"
- iyz="-0.00000306125"
- izz= "0.00018105446" />
+ <inertia ixx= "{scale*0.00007756198}"
+ ixy= "{scale*0.00000149095}"
+ ixz="{-scale*0.00000983385}"
+ iyy= "{scale*0.00019708305}"
+ iyz="{-scale*0.00000306125}"
+ izz= "{scale*0.00018105446}" />
</inertial>
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
@@ -147,12 +148,12 @@
<inertial>
<mass value="0.17389" />
<com xyz="${0.03576} -0.01736 -0.00095" />
- <inertia ixx= "0.00007738410"
- ixy="-0.00000209309"
- ixz="-0.00000836228"
- iyy= "0.00019847383"
- iyz= "0.00000246110"
- izz= "0.00018106988" />
+ <inertia ixx= "{scale*0.00007738410}"
+ ixy="{-scale*0.00000209309}"
+ ixz="{-scale*0.00000836228}"
+ iyy= "{scale*0.00019847383}"
+ iyz= "{scale*0.00000246110}"
+ izz= "{scale*0.00018106988}" />
</inertial>
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
@@ -229,9 +230,9 @@
<inertial>
<mass value="0.04419" />
<com xyz="0.00423 0.00284 0.0" />
- <inertia ixx="0.00000837047" ixy="0.00000583632" ixz="0.0"
- iyy="0.00000987067" iyz="0.0"
- izz="0.00001541768" />
+ <inertia ixx="{scale*0.00000837047}" ixy="{scale*0.00000583632}" ixz="{scale*0.0}"
+ iyy="{scale*0.00000987067}" iyz="{scale*0.0}"
+ izz="{scale*0.00001541768}" />
</inertial>
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
@@ -309,9 +310,9 @@
<inertial>
<mass value="0.04419" />
<com xyz="${0.00423} ${-0.00284} ${0.0}" />
- <inertia ixx="0.00000837047" ixy="-0.00000583632" ixz="0.0"
- iyy="0.00000987067" iyz="0.0"
- izz="0.00001541768" />
+ <inertia ixx="{scale*0.00000837047)" ixy="{-scale*0.00000583632)" ixz="{scale*0.0)"
+ iyy="{scale*0.00000987067)" iyz="{scale*0.0)"
+ izz="{scale*0.00001541768)" />
</inertial>
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
@@ -580,10 +581,10 @@
<actuator name="${side}_gripper_motor" />
<gap_joint name="${side}_gripper_joint" mechanical_reduction="1.0" />
<passive_joint name="${side}_gripper_l_finger_joint" >
- <gazebo_mimic_pid p="0.001" i="0.0" d="0.0" iClamp="0.0" />
+ <gazebo_mimic_pid p="0.0001" i="0.0" d="0.0" iClamp="0.0" />
</passive_joint>
<passive_joint name="${side}_gripper_r_finger_joint" >
- <gazebo_mimic_pid p="0.001" i="0.0" d="0.0" iClamp="0.0" />
+ <gazebo_mimic_pid p="0.0001" i="0.0" d="0.0" iClamp="0.0" />
</passive_joint>
<passive_joint name="${side}_gripper_r_finger_tip_joint" >
<gazebo_mimic_pid p="0.00001" i="0.0" d="0.0" iClamp="0.0" />
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
