[Personalrobots-commit] SF.net SVN: personalrobots:[3390] pkg/trunk

[<hsu...@us...>]

Revision: 3390
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=3390&view=rev
Author:   hsujohnhsu
Date:     2008-08-21 00:42:21 +0000 (Thu, 21 Aug 2008)

Log Message:
-----------
gripper sign error fix.

Modified Paths:
--------------
    pkg/trunk/drivers/simulator/gazebo_plugin/src/test_actuators.cpp
    pkg/trunk/robot_descriptions/wg_robot_description/pr2/controllers.xml

Modified: pkg/trunk/drivers/simulator/gazebo_plugin/src/test_actuators.cpp
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/src/test_actuators.cpp	2008-08-21 00:39:49 UTC (rev 3389)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/src/test_actuators.cpp	2008-08-21 00:42:21 UTC (rev 3390)
@@ -613,22 +613,22 @@
         controller::JointPositionControllerNode* jpc = dynamic_cast<controller::JointPositionControllerNode*>(jc);
         gripperCmd   = jpc->getCommand();
 
-        currentError = math_utils::shortest_angular_distance(dynamic_cast<gazebo::HingeJoint*>((*gji)->gaz_joints_[0])->GetAngle(),gripperCmd);
+        currentError = math_utils::shortest_angular_distance( gripperCmd,dynamic_cast<gazebo::HingeJoint*>((*gji)->gaz_joints_[0])->GetAngle());
         currentCmd   = (*gji)->gaz_gripper_pids_[0]->updatePid(currentError,currentTime-lastTime);
         dynamic_cast<gazebo::HingeJoint*>((*gji)->gaz_joints_[0])->SetParam( dParamVel, currentCmd );
         dynamic_cast<gazebo::HingeJoint*>((*gji)->gaz_joints_[0])->SetParam( dParamFMax, (*gji)->saturationTorque );
 
-        currentError = math_utils::shortest_angular_distance(dynamic_cast<gazebo::HingeJoint*>((*gji)->gaz_joints_[1])->GetAngle(),-gripperCmd);
+        currentError = math_utils::shortest_angular_distance(-gripperCmd,dynamic_cast<gazebo::HingeJoint*>((*gji)->gaz_joints_[1])->GetAngle());
         currentCmd   = (*gji)->gaz_gripper_pids_[1]->updatePid(currentError,currentTime-lastTime);
         dynamic_cast<gazebo::HingeJoint*>((*gji)->gaz_joints_[1])->SetParam( dParamVel, currentCmd );
         dynamic_cast<gazebo::HingeJoint*>((*gji)->gaz_joints_[1])->SetParam( dParamFMax, (*gji)->saturationTorque );
 
-        currentError = math_utils::shortest_angular_distance(dynamic_cast<gazebo::HingeJoint*>((*gji)->gaz_joints_[2])->GetAngle(),-gripperCmd);
+        currentError = math_utils::shortest_angular_distance(-gripperCmd,dynamic_cast<gazebo::HingeJoint*>((*gji)->gaz_joints_[2])->GetAngle());
         currentCmd   = (*gji)->gaz_gripper_pids_[2]->updatePid(currentError,currentTime-lastTime);
         dynamic_cast<gazebo::HingeJoint*>((*gji)->gaz_joints_[2])->SetParam( dParamVel, currentCmd );
         dynamic_cast<gazebo::HingeJoint*>((*gji)->gaz_joints_[2])->SetParam( dParamFMax, (*gji)->saturationTorque );
 
-        currentError = math_utils::shortest_angular_distance(dynamic_cast<gazebo::HingeJoint*>((*gji)->gaz_joints_[3])->GetAngle(),gripperCmd);
+        currentError = math_utils::shortest_angular_distance( gripperCmd,dynamic_cast<gazebo::HingeJoint*>((*gji)->gaz_joints_[3])->GetAngle());
         currentCmd   = (*gji)->gaz_gripper_pids_[3]->updatePid(currentError,currentTime-lastTime);
         dynamic_cast<gazebo::HingeJoint*>((*gji)->gaz_joints_[3])->SetParam( dParamVel, currentCmd );
         dynamic_cast<gazebo::HingeJoint*>((*gji)->gaz_joints_[3])->SetParam( dParamFMax, (*gji)->saturationTorque );

Modified: pkg/trunk/robot_descriptions/wg_robot_description/pr2/controllers.xml
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description/pr2/controllers.xml	2008-08-21 00:39:49 UTC (rev 3389)
+++ pkg/trunk/robot_descriptions/wg_robot_description/pr2/controllers.xml	2008-08-21 00:42:21 UTC (rev 3390)
@@ -209,7 +209,7 @@
   <!-- Special gripper joint -->
   <controller name="gripper_left_controller" topic="gripper_left_controller" type="JointPositionControllerNode">
         <joint name="gripper_left_joint">
-        <pid p="20" d="0" i="0" iClamp="0" />
+        <pid p="1" d="0" i="0" iClamp="0" />
         <gripper_defaults effortLimit="100" velocityLimit="10" />
         <map name="gripper_joints" flag="gazebo">
           <elem key="saturationTorque">100</elem>
@@ -297,7 +297,7 @@
       <!-- Special gripper joint -->
   <controller name="gripper_right_controller" topic="gripper_right_controller" type="JointPositionControllerNode">
         <joint name="gripper_right_joint">
-        <pid p="20" d="0" i="0" iClamp="0" />
+        <pid p="1" d="0" i="0" iClamp="0" />
         <gripper_defaults effortLimit="100" velocityLimit="10" />
         <map name="gripper_joints" flag="gazebo">
           <elem key="saturationTorque">100</elem>


This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.