Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[4922] pkg/trunk
|
From: <hsu...@us...> - 2008-10-02 07:54:49
|
Revision: 4922
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=4922&view=rev
Author: hsujohnhsu
Date: 2008-10-02 07:54:39 +0000 (Thu, 02 Oct 2008)
Log Message:
-----------
proper mass manipulation working, anchor offset was double counted due to recent updates in Gazebo.
returned damping and gains for caster and gripper to pre r4918.
dual link case working.
Modified Paths:
--------------
pkg/trunk/3rdparty/gazebo/gazebo_patch.diff
pkg/trunk/robot_descriptions/gazebo_robot_description/src/urdf2gazebo.cpp
pkg/trunk/robot_descriptions/wg_robot_description/dual_link_test/pr2_dual_link.xml
pkg/trunk/robot_descriptions/wg_robot_description/pr2/controllers.xml
pkg/trunk/robot_descriptions/wg_robot_description/pr2/gazebo/gazebo_joints.xml
pkg/trunk/robot_descriptions/wg_robot_description/pr2/pr2.xml
Modified: pkg/trunk/3rdparty/gazebo/gazebo_patch.diff
===================================================================
--- pkg/trunk/3rdparty/gazebo/gazebo_patch.diff 2008-10-02 06:40:19 UTC (rev 4921)
+++ pkg/trunk/3rdparty/gazebo/gazebo_patch.diff 2008-10-02 07:54:39 UTC (rev 4922)
@@ -369,7 +369,7 @@
}
////////////////////////////////////////////////////////////////////////////////
-@@ -123,6 +144,56 @@
+@@ -123,6 +144,63 @@
this->massP->SetValue( 0.001 );
}
@@ -400,16 +400,23 @@
+ this->mass.c[0] = this->cx;
+ this->mass.c[1] = this->cy;
+ this->mass.c[2] = this->cz;
++
+ this->mass.I[0] = this->ixx;
+ this->mass.I[1] = this->ixy;
+ this->mass.I[2] = this->ixz;
++
+ this->mass.I[3] = this->ixy;
+ this->mass.I[4] = this->iyy;
+ this->mass.I[5] = this->iyz;
++
+ this->mass.I[6] = this->ixz;
+ this->mass.I[7] = this->iyz;
+ this->mass.I[8] = this->izz;
+
++ this->mass.I[9] = 1;
++ this->mass.I[10] = 1;
++ this->mass.I[11] = 1;
++
+ // std::cout << " c[0] " << this->mass.c[0] << std::endl;
+ // std::cout << " c[1] " << this->mass.c[1] << std::endl;
+ // std::cout << " c[2] " << this->mass.c[2] << std::endl;
@@ -426,7 +433,7 @@
this->contact->Load(node);
this->LoadChild(node);
-@@ -396,23 +467,25 @@
+@@ -396,23 +474,24 @@
Pose3d pose;
dQuaternion q;
dMatrix3 r;
@@ -446,7 +453,7 @@
- dQtoR(q,r);
+ dQtoR(q,r); // turn quaternion into rotation matrix
-
+-
+ // this->mass was init to zero at start,
+ // read user specified mass into this->dblMass and dMassAdd in this->mass
this->bodyMass = this->mass;
@@ -575,43 +582,59 @@
// Return the ID of this body
dBodyID Body::GetId() const
{
-@@ -505,8 +590,15 @@
- */
+@@ -506,7 +591,6 @@
void Body::UpdateCoM()
{
-+ // std::cout << " mass x " << this->mass.c[0]
-+ // << " y " << this->mass.c[1]
-+ // << " z " << this->mass.c[2]
-+ // << std::endl;
-+ // comPose.pos.x = this->mass.c[0];
-+ // comPose.pos.y = this->mass.c[1];
-+ // comPose.pos.z = this->mass.c[2];
-+
const dMass *lmass;
- Pose3d oldPose, newPose, pose;
std::map< std::string, Geom* >::iterator giter;
if (!this->bodyId)
-@@ -523,8 +615,10 @@
- dMassAdd( &this->mass, lmass );
+@@ -524,7 +608,13 @@
}
}
-+ return; // Stop pose update, we have full com xyz, I control
++ //return; // Stop pose update, we have full com xyz, I control
++
// Old pose for the CoM
-+ Pose3d oldPose, newPose, pose;
++ Pose3d oldPose, newPose, tmpPose;
++
++ // oldPose is the last comPose
++ // newPose is mass CoM
oldPose = this->comPose;
if (std::isnan(this->mass.c[0]))
-@@ -565,7 +659,7 @@
+@@ -546,19 +636,25 @@
+ {
+ if (giter->second->IsPlaceable())
+ {
++ // FOR GEOMS:
++ // get pose with comPose set to oldPose
+ this->comPose = oldPose;
+- pose = giter->second->GetPose();
++ tmpPose = giter->second->GetPose();
++
++ // get pose with comPose set to newPose
+ this->comPose = newPose;
+- giter->second->SetPose(pose, false);
++ giter->second->SetPose(tmpPose, false);
+ }
+ }
+
+
+- // Fixup the pose of the CoM (ODE body)
++ // FOR BODY: Fixup the pose of the CoM (ODE body)
++ // get pose with comPose set to oldPose
+ this->comPose = oldPose;
+- pose = this->GetPose();
++ tmpPose = this->GetPose();
++ // get pose with comPose set to newPose
this->comPose = newPose;
+- this->SetPose(pose);
++ this->SetPose(tmpPose);
- // My Cheap Hack, to put the center of mass at the origin
-- this->mass.c[0] = this->mass.c[1] = this->mass.c[2] = 0;
-+ //this->mass.c[0] = this->mass.c[1] = this->mass.c[2] = 0;
- // Set the mass matrix
- if (this->mass.mass > 0)
+ // Settle on the new CoM pose
Index: server/physics/ode/ODEPhysics.hh
===================================================================
--- server/physics/ode/ODEPhysics.hh (revision 7049)
@@ -1195,6 +1218,18 @@
this->RTTMode="PBuffer";
}
}
+Index: server/gui/StatusBar.cc
+===================================================================
+--- server/gui/StatusBar.cc (revision 7049)
++++ server/gui/StatusBar.cc (working copy)
+@@ -25,6 +25,7 @@
+ */
+
+ #include <stdio.h>
++#include <string.h>
+ #include <FL/Fl_Value_Output.H>
+ #include <FL/Fl_Output.H>
+ #include <FL/Fl_Button.H>
Index: server/Model.cc
===================================================================
--- server/Model.cc (revision 7049)
@@ -1266,18 +1301,6 @@
return this->UpdateChild();
}
-Index: server/gui/StatusBar.cc
-===================================================================
---- server/gui/StatusBar.cc (revision 7049)
-+++ server/gui/StatusBar.cc (working copy)
-@@ -25,6 +25,7 @@
- */
-
- #include <stdio.h>
-+#include <string.h>
- #include <FL/Fl_Value_Output.H>
- #include <FL/Fl_Output.H>
- #include <FL/Fl_Button.H>
Index: server/World.hh
===================================================================
--- server/World.hh (revision 7049)
Modified: pkg/trunk/robot_descriptions/gazebo_robot_description/src/urdf2gazebo.cpp
===================================================================
--- pkg/trunk/robot_descriptions/gazebo_robot_description/src/urdf2gazebo.cpp 2008-10-02 06:40:19 UTC (rev 4921)
+++ pkg/trunk/robot_descriptions/gazebo_robot_description/src/urdf2gazebo.cpp 2008-10-02 07:54:39 UTC (rev 4922)
@@ -315,7 +315,7 @@
for (int j = 0 ; j < 3 ; ++j)
{
// undo Gazebo's shift of object anchor to geom xyz, stay in body cs
- tmpAnchor[j] = (link->joint->anchor)[j] - 0.0*(link->inertial->com)[j] - 0.0*(link->collision->xyz)[j]; /// @todo compensate for gazebo's error. John is fixing this one
+ tmpAnchor[j] = (link->joint->anchor)[j] - 1.0*(link->inertial->com)[j] - 0.0*(link->collision->xyz)[j]; /// @todo compensate for gazebo's error. John is fixing this one
}
addKeyValue(joint, "anchorOffset", values2str(3, tmpAnchor));
Modified: pkg/trunk/robot_descriptions/wg_robot_description/dual_link_test/pr2_dual_link.xml
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description/dual_link_test/pr2_dual_link.xml 2008-10-02 06:40:19 UTC (rev 4921)
+++ pkg/trunk/robot_descriptions/wg_robot_description/dual_link_test/pr2_dual_link.xml 2008-10-02 07:54:39 UTC (rev 4922)
@@ -13,7 +13,7 @@
<!-- joint blocks -->
<joint name="link2_joint" type="revolute" >
<axis xyz="0 1 0" />
- <anchor xyz="-0.5 0 0" />
+ <anchor xyz="0.0 0 0" />
<limit min="-3.14159" max="3.14159" effort="100" velocity="5" />
<calibration values="1.5 -1 " />
</joint>
@@ -81,15 +81,15 @@
<link name="link2">
<parent name="link1" />
- <origin xyz="0.5 0 0" rpy="0 0 0" />
+ <origin xyz="0.0 0 0" rpy="0 0 0" />
<joint name="link2_joint" />
<inertial >
<mass value="10" />
- <com xyz="0 0 0" />
- <inertia ixx="0.0" ixy="0.0" ixz="0.0" iyy="0.0" iyz="0.0" izz="0.0" />
+ <com xyz="1.0 0 0" />
+ <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="100.0" iyz="0.0" izz="1.0" />
</inertial>
<visual >
- <origin xyz="0.0 0 0" rpy="0 0 0" />
+ <origin xyz="0.5 0 0" rpy="0 0 0" />
<map name="gazebo_material" flag="gazebo">
<elem key="material">Gazebo/Blue</elem>
</map>
@@ -98,7 +98,7 @@
</geometry>
</visual>
<collision >
- <origin xyz="0.0 0 0" rpy="0.0 0.0 0.0 " />
+ <origin xyz="0.5 0 0" rpy="0.0 0.0 0.0 " />
<geometry name="link2_collision_geom">
<box size="1.0 0.1 0.1" />
</geometry>
Modified: pkg/trunk/robot_descriptions/wg_robot_description/pr2/controllers.xml
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description/pr2/controllers.xml 2008-10-02 06:40:19 UTC (rev 4921)
+++ pkg/trunk/robot_descriptions/wg_robot_description/pr2/controllers.xml 2008-10-02 07:54:39 UTC (rev 4922)
@@ -10,63 +10,63 @@
</map>
<controller name="wheel_front_left_l_controller" topic="wheel_front_left_l_controller" type="JointVelocityControllerNode">
<joint name="wheel_front_left_l_joint" >
- <pid p="0.5" d="0" i="0.2" iClamp="2" />
+ <pid p="2.0" d="0" i="0.2" iClamp="2" />
</joint>
</controller>
<controller name="wheel_front_left_r_controller" topic="wheel_front_left_r_controller" type="JointVelocityControllerNode">
<joint name="wheel_front_left_r_joint" >
- <pid p="0.5" d="0" i="0.2" iClamp="2" />
+ <pid p="2.0" d="0" i="0.2" iClamp="2" />
</joint>
</controller>
<controller name="wheel_front_right_l_controller" topic="wheel_front_right_l_controller" type="JointVelocityControllerNode">
<joint name="wheel_front_right_l_joint" >
- <pid p="0.5" d="0" i="0.2" iClamp="2" />
+ <pid p="2.0" d="0" i="0.2" iClamp="2" />
</joint>
</controller>
<controller name="wheel_front_right_r_controller" topic="wheel_front_right_r_controller" type="JointVelocityControllerNode">
<joint name="wheel_front_right_r_joint" >
- <pid p="0.5" d="0" i="0.2" iClamp="2" />
+ <pid p="2.0" d="0" i="0.2" iClamp="2" />
</joint>
</controller>
<controller name="wheel_rear_left_l_controller" topic="wheel_rear_left_l_controller" type="JointVelocityControllerNode">
<joint name="wheel_rear_left_l_joint" >
- <pid p="0.5" d="0" i="0.2" iClamp="2" />
+ <pid p="2.0" d="0" i="0.2" iClamp="2" />
</joint>
</controller>
<controller name="wheel_rear_left_r_controller" topic="wheel_rear_left_r_controller" type="JointVelocityControllerNode">
<joint name="wheel_rear_left_r_joint" >
- <pid p="0.5" d="0" i="0.2" iClamp="2" />
+ <pid p="2.0" d="0" i="0.2" iClamp="2" />
</joint>
</controller>
<controller name="wheel_rear_right_l_controller" topic="wheel_rear_right_l_controller" type="JointVelocityControllerNode">
<joint name="wheel_rear_right_l_joint" >
- <pid p="0.5" d="0" i="0.2" iClamp="2" />
+ <pid p="2.0" d="0" i="0.2" iClamp="2" />
</joint>
</controller>
<controller name="wheel_rear_right_r_controller" topic="wheel_rear_right_r_controller" type="JointVelocityControllerNode">
<joint name="wheel_rear_right_r_joint" >
- <pid p="0.5" d="0" i="0.2" iClamp="2" />
+ <pid p="2.0" d="0" i="0.2" iClamp="2" />
</joint>
</controller>
<controller name="caster_front_left_controller" topic="caster_front_left_controller" type="JointVelocityControllerNode">
<joint name="caster_front_left_joint" >
- <pid p="0.1" d="0.05" i="0.05" iClamp="0.5" />
+ <pid p="3" d="0" i="0.1" iClamp="4" />
</joint>
</controller>
<controller name="caster_front_right_controller" topic="caster_front_right_controller" type="JointVelocityControllerNode">
<joint name="caster_front_right_joint" >
- <pid p="0.1" d="0.05" i="0.05" iClamp="0.5" />
+ <pid p="3" d="0" i="0.1" iClamp="4" />
</joint>
</controller>
<controller name="caster_rear_left_controller" topic="caster_rear_left_controller" type="JointVelocityControllerNode">
<joint name="caster_rear_left_joint" >
- <pid p="0.1" d="0.05" i="0.05" iClamp="0.5" />
+ <pid p="3" d="0" i="0.1" iClamp="4" />
</joint>
</controller>
<controller name="caster_rear_right_controller" topic="caster_rear_right_controller" type="JointVelocityControllerNode">
<joint name="caster_rear_right_joint" >
- <pid p="0.1" d="0.05" i="0.05" iClamp="0.5" />
+ <pid p="3" d="0" i="0.1" iClamp="4" />
</joint>
</controller>
@@ -130,8 +130,8 @@
<controller name="gripper_left_controller" topic="gripper_left_controller" type="JointPositionControllerNode">
<listen_topic name="left_gripper_commands" />
<joint name="finger_l_left_joint">
- <pid p="100" d="10" i="1" iClamp="10" />
- <gripper_defaults effortLimit="1000" velocityLimit="10" />
+ <pid p="10" d="0" i="0" iClamp="0" />
+ <gripper_defaults effortLimit="100" velocityLimit="10" />
</joint>
</controller>
<!-- ========================================= -->
@@ -184,8 +184,8 @@
<controller name="gripper_right_controller" topic="gripper_right_controller" type="JointPositionControllerNode">
<listen_topic name="right_gripper_commands" />
<joint name="finger_l_right_joint">
- <pid p="100" d="10" i="1" iClamp="10" />
- <gripper_defaults effortLimit="1000" velocityLimit="10" />
+ <pid p="10" d="0" i="0" iClamp="0" />
+ <gripper_defaults effortLimit="100" velocityLimit="10" />
</joint>
</controller>
<!-- head and above array -->
Modified: pkg/trunk/robot_descriptions/wg_robot_description/pr2/gazebo/gazebo_joints.xml
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description/pr2/gazebo/gazebo_joints.xml 2008-10-02 06:40:19 UTC (rev 4921)
+++ pkg/trunk/robot_descriptions/wg_robot_description/pr2/gazebo/gazebo_joints.xml 2008-10-02 07:54:39 UTC (rev 4922)
@@ -62,16 +62,16 @@
</joint>
<!-- Special gripper joint -->
<joint name="finger_l_left_joint" gripper_controller="gripper_left_controller" abstract="gripper" effortLimit="1000" velocityLimit="100">
- <explicitDampingCoefficient>10</explicitDampingCoefficient>
+ <explicitDampingCoefficient>0.1</explicitDampingCoefficient>
</joint>
<joint name="finger_r_left_joint" gripper_controller="gripper_left_controller" abstract="gripper" effortLimit="1000" velocityLimit="100">
- <explicitDampingCoefficient>10</explicitDampingCoefficient>
+ <explicitDampingCoefficient>0.1</explicitDampingCoefficient>
</joint>
<joint name="finger_tip_l_left_joint" gripper_controller="gripper_left_controller" abstract="gripper" effortLimit="1000" velocityLimit="100">
- <explicitDampingCoefficient>10</explicitDampingCoefficient>
+ <explicitDampingCoefficient>0.1</explicitDampingCoefficient>
</joint>
<joint name="finger_tip_r_left_joint" gripper_controller="gripper_left_controller" abstract="gripper" effortLimit="1000" velocityLimit="100">
- <explicitDampingCoefficient>10</explicitDampingCoefficient>
+ <explicitDampingCoefficient>0.1</explicitDampingCoefficient>
</joint>
<!-- ========================================= -->
<!-- right arm array -->
@@ -98,16 +98,16 @@
</joint>
<!-- Special gripper joint -->
<joint name="finger_l_right_joint" gripper_controller="gripper_right_controller" abstract="gripper" effortLimit="1000" velocityLimit="100">
- <explicitDampingCoefficient>10</explicitDampingCoefficient>
+ <explicitDampingCoefficient>0.1</explicitDampingCoefficient>
</joint>
<joint name="finger_r_right_joint" gripper_controller="gripper_right_controller" abstract="gripper" effortLimit="1000" velocityLimit="100">
- <explicitDampingCoefficient>10</explicitDampingCoefficient>
+ <explicitDampingCoefficient>0.1</explicitDampingCoefficient>
</joint>
<joint name="finger_tip_l_right_joint" gripper_controller="gripper_right_controller" abstract="gripper" effortLimit="1000" velocityLimit="100">
- <explicitDampingCoefficient>10</explicitDampingCoefficient>
+ <explicitDampingCoefficient>0.1</explicitDampingCoefficient>
</joint>
<joint name="finger_tip_r_right_joint" gripper_controller="gripper_right_controller" abstract="gripper" effortLimit="1000" velocityLimit="100">
- <explicitDampingCoefficient>10</explicitDampingCoefficient>
+ <explicitDampingCoefficient>0.1</explicitDampingCoefficient>
</joint>
<!-- head and above array -->
<joint name="head_pan_joint" >
Modified: pkg/trunk/robot_descriptions/wg_robot_description/pr2/pr2.xml
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description/pr2/pr2.xml 2008-10-02 06:40:19 UTC (rev 4921)
+++ pkg/trunk/robot_descriptions/wg_robot_description/pr2/pr2.xml 2008-10-02 07:54:39 UTC (rev 4922)
@@ -360,7 +360,7 @@
</const_block>
<const_block name="pr2_shoulder_pitch_collision">
- <origin xyz="shoulder_pitch_length/2 0 0 " rpy="M_PI/2 0.0 0.0 " /> <!-- location defined with respect to the link origin in a local coordinate frame -->
+ <origin xyz="0 0 0" rpy="M_PI/2 0.0 0.0 " /> <!-- location defined with respect to the link origin in a local coordinate frame -->
<!-- All angles always in radians, yaw about Z axis, pitch about the Y axis and roll about the X axis -->
<geometry name="pr2_shoulder_pitch_collision_geom">
<cylinder radius="shoulder_pitch_radius" length="shoulder_pitch_length" />
@@ -545,7 +545,7 @@
<const_block name="pr2_finger_l_joint">
<axis xyz=" 0 0 1 " /> <!-- direction of the joint in a global coordinate frame -->
<anchor xyz="0 0 0 " /> <!-- point on the joint relative to the parent's anchor in a global coordinate frame -->
- <limit effort="1000" velocity="5" />
+ <limit effort="10" velocity="5" />
<calibration values="1.5 -1 " /> <!-- Calibration stop/flag indicating location and then direction -->
</const_block>
@@ -580,7 +580,7 @@
<const_block name="pr2_finger_tip_l_joint">
<axis xyz=" 0 0 1 " /> <!-- direction of the joint in a global coordinate frame -->
<anchor xyz="0 0 0 " /> <!-- point on the joint relative to the parent's anchor in a global coordinate frame -->
- <limit effort="1000" velocity="5" />
+ <limit effort="10" velocity="5" />
<calibration values="1.5 -1 " /> <!-- Calibration stop/flag indicating location and then direction -->
</const_block>
@@ -615,7 +615,7 @@
<axis xyz=" 0 0 1 " /> <!-- direction of the joint in a global coordinate frame -->
<anchor xyz="0 0 0 " /> <!-- point on the joint relative to the parent's anchor in a global coordinate frame -->
<calibration values="1.5 -1 " /> <!-- Calibration stop/flag indicating location and then direction -->
- <limit effort="1000" velocity="10" min="-100" max="100" /><!-- FIXME -->
+ <limit effort="10" velocity="10" min="-100" max="100" /><!-- FIXME -->
</const_block>
<const_block name="pr2_finger_r_inertial">
@@ -649,7 +649,7 @@
<axis xyz=" 0 0 1 " /> <!-- direction of the joint in a global coordinate frame -->
<anchor xyz="0 0 0 " /> <!-- point on the joint relative to the parent's anchor in a global coordinate frame -->
<calibration values="1.5 -1 " /> <!-- Calibration stop/flag indicating location and then direction -->
- <limit effort="1000" velocity="10" min="-100" max="100" /><!-- FIXME -->
+ <limit effort="10" velocity="10" min="-100" max="100" /><!-- FIXME -->
</const_block>
<const_block name="pr2_finger_tip_r_inertial">
@@ -1206,7 +1206,7 @@
<link name="shoulder_pitch_left">
<parent name=" shoulder_pan_left" />
<!-- rotation of a local coordinate frame attached to the link with respect to the parent's frame -->
- <origin xyz=" shoulder_pan_shoulder_pitch_offset_x 0 0 " rpy=" 0 0 0 " /> <!-- position of a local coordinate frame attached to the link with respect to parent's frame-->
+ <origin xyz="shoulder_pan_shoulder_pitch_offset_x 0 0 " rpy=" 0 0 0 " /> <!-- position of a local coordinate frame attached to the link with respect to parent's frame-->
<joint name="shoulder_pitch_left_joint" />
<inertial >
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
