Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[4918] pkg/trunk
|
From: <hsu...@us...> - 2008-10-02 02:26:38
|
Revision: 4918
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=4918&view=rev
Author: hsujohnhsu
Date: 2008-10-02 02:26:26 +0000 (Thu, 02 Oct 2008)
Log Message:
-----------
* fix double anchor offset in gazebo, Inertia updates. validation pending, setting up test case.
* update gains in gripper and base. update effort limits in gripper.
Modified Paths:
--------------
pkg/trunk/3rdparty/gazebo/gazebo_patch.diff
pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_dual_link.world
pkg/trunk/robot_descriptions/wg_robot_description/dual_link_test/gazebo/gazebo_joints_dual_link.xml
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 00:43:55 UTC (rev 4917)
+++ pkg/trunk/3rdparty/gazebo/gazebo_patch.diff 2008-10-02 02:26:26 UTC (rev 4918)
@@ -302,18 +302,6 @@
};
/// \}
-Index: server/physics/HingeJoint.hh
-===================================================================
---- server/physics/HingeJoint.hh (revision 7049)
-+++ server/physics/HingeJoint.hh (working copy)
-@@ -126,6 +126,7 @@
- private: ParamT<Vector3> *axisP;
- private: ParamT<Angle> *loStopP;
- private: ParamT<Angle> *hiStopP;
-+ private: ParamT<Vector3> *anchorOffsetP;
- };
- /// \}
-
Index: server/physics/CylinderGeom.cc
===================================================================
--- server/physics/CylinderGeom.cc (revision 7049)
@@ -381,7 +369,7 @@
}
////////////////////////////////////////////////////////////////////////////////
-@@ -123,6 +144,30 @@
+@@ -123,6 +144,56 @@
this->massP->SetValue( 0.001 );
}
@@ -408,11 +396,42 @@
+ this->ixz = this->ixzP->GetValue();
+ this->iyz = this->iyzP->GetValue();
+
++ // setup this->mass as well
++ 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;
+
++ // 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;
++ // std::cout << " I[0] " << this->mass.I[0] << std::endl;
++ // std::cout << " I[1] " << this->mass.I[1] << std::endl;
++ // std::cout << " I[2] " << this->mass.I[2] << std::endl;
++ // std::cout << " I[3] " << this->mass.I[3] << std::endl;
++ // std::cout << " I[4] " << this->mass.I[4] << std::endl;
++ // std::cout << " I[5] " << this->mass.I[5] << std::endl;
++ // 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;
++
this->contact->Load(node);
this->LoadChild(node);
-@@ -401,18 +446,21 @@
+@@ -396,23 +467,25 @@
+ Pose3d pose;
+ dQuaternion q;
+ dMatrix3 r;
+- dMass bodyMass;
+
if (!this->placeable)
return NULL;
@@ -556,43 +575,43 @@
// Return the ID of this body
dBodyID Body::GetId() const
{
-@@ -505,6 +590,7 @@
+@@ -505,8 +590,15 @@
*/
void Body::UpdateCoM()
{
-+ return;
++ // 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;
+- Pose3d oldPose, newPose, pose;
std::map< std::string, Geom* >::iterator giter;
-Index: server/physics/HingeJoint.cc
-===================================================================
---- server/physics/HingeJoint.cc (revision 7049)
-+++ server/physics/HingeJoint.cc (working copy)
-@@ -44,6 +44,7 @@
- this->axisP = new ParamT<Vector3>("axis",Vector3(0,1,0), 1);
- this->loStopP = new ParamT<Angle>("lowStop",-M_PI,0);
- this->hiStopP = new ParamT<Angle>("highStop",M_PI,0);
-+ this->anchorOffsetP = new ParamT<Vector3>("anchorOffset",Vector3(0,0,0),0);
- Param::End();
- }
-@@ -64,6 +65,7 @@
- this->axisP->Load(node);
- this->loStopP->Load(node);
- this->hiStopP->Load(node);
-+ this->anchorOffsetP->Load(node);
+ if (!this->bodyId)
+@@ -523,8 +615,10 @@
+ dMassAdd( &this->mass, lmass );
+ }
+ }
++ return; // Stop pose update, we have full com xyz, I control
- // Perform this three step ordering to ensure the parameters are set
- // properly. This is taken from the ODE wiki.
-@@ -133,7 +135,7 @@
- // Set the anchor point
- void HingeJoint::SetAnchor( const Vector3 &anchor )
- {
-- dJointSetHingeAnchor( this->jointId, anchor.x, anchor.y, anchor.z );
-+ dJointSetHingeAnchor( this->jointId, anchor.x+anchorOffsetP->GetValue().x, anchor.y+anchorOffsetP->GetValue().y, anchor.z+anchorOffsetP->GetValue().z );
- }
+ // Old pose for the CoM
++ Pose3d oldPose, newPose, pose;
+ oldPose = this->comPose;
+ if (std::isnan(this->mass.c[0]))
+@@ -565,7 +659,7 @@
+ this->comPose = newPose;
+ // 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)
Index: server/physics/ode/ODEPhysics.hh
===================================================================
--- server/physics/ode/ODEPhysics.hh (revision 7049)
Modified: pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_dual_link.world
===================================================================
--- pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_dual_link.world 2008-10-02 00:43:55 UTC (rev 4917)
+++ pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_dual_link.world 2008-10-02 02:26:26 UTC (rev 4918)
@@ -38,26 +38,12 @@
<size>1024 800</size>
<pos>0 0</pos>
<frames>
- <row height="50%">
- <camera width="50%">
+ <row height="100%">
+ <camera width="100%">
<xyz> .25 1.5 1.0</xyz>
<rpy> 0 0 -90</rpy>
</camera>
- <camera width="50%">
- <xyz>.25 0 2.5</xyz>
- <rpy> 0 90 0</rpy>
- </camera>
</row>
- <row height="50%">
- <camera width="50%">
- <xyz>-1 0 1</xyz>
- <rpy> 0 0 0</rpy>
- </camera>
- <camera width="50%">
- <xyz>1.5 0 1</xyz>
- <rpy>0 0 180</rpy>
- </camera>
- </row>
</frames>
</rendering:gui>
Modified: pkg/trunk/robot_descriptions/wg_robot_description/dual_link_test/gazebo/gazebo_joints_dual_link.xml
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description/dual_link_test/gazebo/gazebo_joints_dual_link.xml 2008-10-02 00:43:55 UTC (rev 4917)
+++ pkg/trunk/robot_descriptions/wg_robot_description/dual_link_test/gazebo/gazebo_joints_dual_link.xml 2008-10-02 02:26:26 UTC (rev 4918)
@@ -3,6 +3,6 @@
<explicitDampingCoefficient>1</explicitDampingCoefficient>
</joint>
<joint name="link2_joint" >
- <explicitDampingCoefficient>1</explicitDampingCoefficient>
+ <explicitDampingCoefficient>0</explicitDampingCoefficient>
</joint>
</robot>
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 00:43:55 UTC (rev 4917)
+++ pkg/trunk/robot_descriptions/wg_robot_description/dual_link_test/pr2_dual_link.xml 2008-10-02 02:26:26 UTC (rev 4918)
@@ -13,7 +13,7 @@
<!-- joint blocks -->
<joint name="link2_joint" type="revolute" >
<axis xyz="0 1 0" />
- <anchor xyz="0 0 0" />
+ <anchor xyz="-0.5 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 0 0" rpy="0 0 0" />
+ <origin xyz="0.5 0 0" rpy="0 0 0" />
<joint name="link2_joint" />
<inertial >
<mass value="10" />
- <com xyz="0.0 0 0" />
- <inertia ixx="0.1" ixy="0.0" ixz="0.0" iyy="0.1" iyz="0.0" izz="0.1" />
+ <com xyz="0 0 0" />
+ <inertia ixx="0.0" ixy="0.0" ixz="0.0" iyy="0.0" iyz="0.0" izz="0.0" />
</inertial>
<visual >
- <origin xyz="0.5 0 0" rpy="0 0 0" />
+ <origin xyz="0.0 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.5 0 0" rpy="0.0 0.0 0.0 " />
+ <origin xyz="0.0 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 00:43:55 UTC (rev 4917)
+++ pkg/trunk/robot_descriptions/wg_robot_description/pr2/controllers.xml 2008-10-02 02:26:26 UTC (rev 4918)
@@ -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="2.0" d="0" i="0.2" iClamp="2" />
+ <pid p="0.5" 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="2.0" d="0" i="0.2" iClamp="2" />
+ <pid p="0.5" 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="2.0" d="0" i="0.2" iClamp="2" />
+ <pid p="0.5" 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="2.0" d="0" i="0.2" iClamp="2" />
+ <pid p="0.5" 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="2.0" d="0" i="0.2" iClamp="2" />
+ <pid p="0.5" 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="2.0" d="0" i="0.2" iClamp="2" />
+ <pid p="0.5" 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="2.0" d="0" i="0.2" iClamp="2" />
+ <pid p="0.5" 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="2.0" d="0" i="0.2" iClamp="2" />
+ <pid p="0.5" 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="3" d="0" i="0.1" iClamp="4" />
+ <pid p="0.1" d="0.05" i="0.05" iClamp="0.5" />
</joint>
</controller>
<controller name="caster_front_right_controller" topic="caster_front_right_controller" type="JointVelocityControllerNode">
<joint name="caster_front_right_joint" >
- <pid p="3" d="0" i="0.1" iClamp="4" />
+ <pid p="0.1" d="0.05" i="0.05" iClamp="0.5" />
</joint>
</controller>
<controller name="caster_rear_left_controller" topic="caster_rear_left_controller" type="JointVelocityControllerNode">
<joint name="caster_rear_left_joint" >
- <pid p="3" d="0" i="0.1" iClamp="4" />
+ <pid p="0.1" d="0.05" i="0.05" iClamp="0.5" />
</joint>
</controller>
<controller name="caster_rear_right_controller" topic="caster_rear_right_controller" type="JointVelocityControllerNode">
<joint name="caster_rear_right_joint" >
- <pid p="3" d="0" i="0.1" iClamp="4" />
+ <pid p="0.1" d="0.05" i="0.05" iClamp="0.5" />
</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="10" d="0" i="0" iClamp="0" />
- <gripper_defaults effortLimit="100" velocityLimit="10" />
+ <pid p="100" d="10" i="1" iClamp="10" />
+ <gripper_defaults effortLimit="1000" 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="10" d="0" i="0" iClamp="0" />
- <gripper_defaults effortLimit="100" velocityLimit="10" />
+ <pid p="100" d="10" i="1" iClamp="10" />
+ <gripper_defaults effortLimit="1000" 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 00:43:55 UTC (rev 4917)
+++ pkg/trunk/robot_descriptions/wg_robot_description/pr2/gazebo/gazebo_joints.xml 2008-10-02 02:26:26 UTC (rev 4918)
@@ -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>0.1</explicitDampingCoefficient>
+ <explicitDampingCoefficient>10</explicitDampingCoefficient>
</joint>
<joint name="finger_r_left_joint" gripper_controller="gripper_left_controller" abstract="gripper" effortLimit="1000" velocityLimit="100">
- <explicitDampingCoefficient>0.1</explicitDampingCoefficient>
+ <explicitDampingCoefficient>10</explicitDampingCoefficient>
</joint>
<joint name="finger_tip_l_left_joint" gripper_controller="gripper_left_controller" abstract="gripper" effortLimit="1000" velocityLimit="100">
- <explicitDampingCoefficient>0.1</explicitDampingCoefficient>
+ <explicitDampingCoefficient>10</explicitDampingCoefficient>
</joint>
<joint name="finger_tip_r_left_joint" gripper_controller="gripper_left_controller" abstract="gripper" effortLimit="1000" velocityLimit="100">
- <explicitDampingCoefficient>0.1</explicitDampingCoefficient>
+ <explicitDampingCoefficient>10</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>0.1</explicitDampingCoefficient>
+ <explicitDampingCoefficient>10</explicitDampingCoefficient>
</joint>
<joint name="finger_r_right_joint" gripper_controller="gripper_right_controller" abstract="gripper" effortLimit="1000" velocityLimit="100">
- <explicitDampingCoefficient>0.1</explicitDampingCoefficient>
+ <explicitDampingCoefficient>10</explicitDampingCoefficient>
</joint>
<joint name="finger_tip_l_right_joint" gripper_controller="gripper_right_controller" abstract="gripper" effortLimit="1000" velocityLimit="100">
- <explicitDampingCoefficient>0.1</explicitDampingCoefficient>
+ <explicitDampingCoefficient>10</explicitDampingCoefficient>
</joint>
<joint name="finger_tip_r_right_joint" gripper_controller="gripper_right_controller" abstract="gripper" effortLimit="1000" velocityLimit="100">
- <explicitDampingCoefficient>0.1</explicitDampingCoefficient>
+ <explicitDampingCoefficient>10</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 00:43:55 UTC (rev 4917)
+++ pkg/trunk/robot_descriptions/wg_robot_description/pr2/pr2.xml 2008-10-02 02:26:26 UTC (rev 4918)
@@ -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="10" velocity="5" />
+ <limit effort="1000" 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="10" velocity="5" />
+ <limit effort="1000" 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="10" velocity="10" min="-100" max="100" /><!-- FIXME -->
+ <limit effort="1000" 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="10" velocity="10" min="-100" max="100" /><!-- FIXME -->
+ <limit effort="1000" velocity="10" min="-100" max="100" /><!-- FIXME -->
</const_block>
<const_block name="pr2_finger_tip_r_inertial">
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
