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

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

Revision: 3456
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=3456&view=rev
Author:   hsujohnhsu
Date:     2008-08-22 00:51:53 +0000 (Fri, 22 Aug 2008)

Log Message:
-----------
kp_speed changed to match that of the hardware (10.0), and base controller is behaving better.
update some comments in test actuator.

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-22 00:49:26 UTC (rev 3455)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/src/test_actuators.cpp	2008-08-22 00:51:53 UTC (rev 3456)
@@ -898,31 +898,92 @@
     this->lock.unlock();
   }
 
+
   void
+  TestActuators::CmdBaseVelReceived()
+  {
+    this->lock.lock();
+    controller::Controller* cc = mc_.getControllerByName( "base_controller" );
+    controller::BaseControllerNode* bc = dynamic_cast<controller::BaseControllerNode*>(cc);
+    bc->setCommand(velMsg.vx,0.0,velMsg.vw);
+    this->lock.unlock();
+  }
+
+
+
+
+
+  void
   TestActuators::CmdLeftarmcartesianReceived()
   {
     this->lock.lock();
+
+    KDL::Frame f;
+    for(int i = 0; i < 9; i++) {
+      f.M.data[i] = cmd_leftarmcartesian.rot[i];
+    }
+    for(int i = 0; i < 3; i++) {
+      f.p.data[i] = cmd_leftarmcartesian.trans[i];
+    }
+
+    bool reachable;
+    //this->PR2Copy->SetArmCartesianPosition(PR2::PR2_LEFT_ARM,f, reachable);
+
     this->lock.unlock();
   }
   void
   TestActuators::CmdRightarmcartesianReceived()
   {
     this->lock.lock();
+
+    KDL::Frame f;
+    for(int i = 0; i < 9; i++) {
+      f.M.data[i] = cmd_rightarmcartesian.rot[i];
+    }
+    for(int i = 0; i < 3; i++) {
+      f.p.data[i] = cmd_rightarmcartesian.trans[i];
+    }
+
+    bool reachable;
+    //this->PR2Copy->SetArmCartesianPosition(PR2::PR2_RIGHT_ARM,f, reachable);
+
     this->lock.unlock();
   }
 
+  bool TestActuators::SetRightArmCartesian(rosgazebo::MoveCartesian::request &req, rosgazebo::MoveCartesian::response &res)
+  {
+    this->lock.lock();
+    KDL::Frame f;
+    for(int i = 0; i < 9; i++)
+      f.M.data[i] = req.e.rot[i];
 
-  void
-  TestActuators::CmdBaseVelReceived()
+    for(int i = 0; i < 3; i++)
+      f.p.data[i] = req.e.trans[i];
+
+          bool reachable;
+    this->PR2Copy->SetArmCartesianPosition(PR2::PR2_RIGHT_ARM,f,reachable);
+          res.reachable = (reachable==false) ? -1 : 0;
+
+    this->lock.unlock();
+          return true;
+  }
+
+  bool TestActuators::OperateRightGripper(rosgazebo::GripperCmd::request &req, rosgazebo::GripperCmd::response &res)
   {
+          this->lock.lock();
+          this->PR2Copy->hw.SetJointServoCmd(PR2::ARM_R_GRIPPER_GAP, req.gap, 0);
+          this->lock.unlock();
+          return true;
+  }
+
+  bool TestActuators::reset_IK_guess(rosgazebo::VoidVoid::request &req, rosgazebo::VoidVoid::response &res)
+  {
     this->lock.lock();
-    controller::Controller* cc = mc_.getControllerByName( "base_controller" );
-    controller::BaseControllerNode* bc = dynamic_cast<controller::BaseControllerNode*>(cc);
-    bc->setCommand(velMsg.vx,0.0,velMsg.vw);
+          this->PR2Copy->GetArmJointPositionCmd(PR2::PR2_RIGHT_ARM, *(this->PR2Copy->right_arm_chain_->q_IK_guess));
     this->lock.unlock();
+          return true;
   }
 
-
   void
   TestActuators::PublishFrameTransforms()
   {
@@ -941,11 +1002,12 @@
     controller::BaseControllerNode* bc = dynamic_cast<controller::BaseControllerNode*>(cc);
     bc->getOdometry(x,y,yaw,vx,vy,vyaw);
 
+    // FIXME: z, roll, pitch not accounted for
     tfs->sendInverseEuler("FRAMEID_ODOM",
                  "base",
                  x,
                  y,
-                 0, //z - base_center_offset_z, /* get infor from xml: half height of base box */
+                 z, //z - base_center_offset_z, /* get infor from xml: half height of base box */
                  yaw,
                  pitch,
                  roll,
@@ -979,7 +1041,7 @@
                  "base",
                  base_torso_offset_x,
                  base_torso_offset_y,
-                 spine_height+base_torso_offset_z, /* FIXME: spine elevator not accounted for */
+                 spine_height+base_torso_offset_z,
                  0.0,
                  0.0,
                  0.0,

Modified: pkg/trunk/robot_descriptions/wg_robot_description/pr2/controllers.xml
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description/pr2/controllers.xml	2008-08-22 00:49:26 UTC (rev 3455)
+++ pkg/trunk/robot_descriptions/wg_robot_description/pr2/controllers.xml	2008-08-22 00:51:53 UTC (rev 3456)
@@ -5,7 +5,7 @@
 
   <controller name="base_controller" topic="base_controller" type="BaseControllerNode">
     <map name="velocity_control" flag="base_control">
-      <elem key="kp_speed">1.0</elem>
+      <elem key="kp_speed">10.0</elem>
     </map>
     <controller name="caster_front_left_controller" topic="caster_front_left_controller" type="JointVelocityControllerNode">
         <joint name="caster_front_left_joint" >


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