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

[<stu...@us...>]

Revision: 5458
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=5458&view=rev
Author:   stuglaser
Date:     2008-10-16 23:28:55 +0000 (Thu, 16 Oct 2008)

Log Message:
-----------
Fixed velocity computations in FK

Modified Paths:
--------------
    pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_velocity_controller.cpp
    pkg/trunk/mechanism/mechanism_model/src/link.cpp

Modified: pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_velocity_controller.cpp
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_velocity_controller.cpp	2008-10-16 23:28:38 UTC (rev 5457)
+++ pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_velocity_controller.cpp	2008-10-16 23:28:55 UTC (rev 5458)
@@ -86,17 +86,15 @@
   effort_.command_[1] = -pid_y_.updatePid(error.y(), time - last_time_);
   effort_.command_[2] = -pid_z_.updatePid(error.z(), time - last_time_);
 
-  //effort_.update();
+  effort_.update();
 
   last_time_ = time;
 }
 
 void CartesianVelocityController::getTipVelocity(tf::Vector3 *v)
 {
-  tf::Vector3 abs_position =
-    tip_->abs_position_ + quatRotate(tip_->abs_orientation_, effort_.offset_);
-
-  *v = tip_->abs_velocity_ + cross(tip_->abs_rot_velocity_, abs_position);
+  *v = tip_->abs_velocity_ + cross(tip_->abs_rot_velocity_,
+                                   quatRotate(tip_->abs_orientation_, effort_.offset_));
 }
 
 

Modified: pkg/trunk/mechanism/mechanism_model/src/link.cpp
===================================================================
--- pkg/trunk/mechanism/mechanism_model/src/link.cpp	2008-10-16 23:28:38 UTC (rev 5457)
+++ pkg/trunk/mechanism/mechanism_model/src/link.cpp	2008-10-16 23:28:55 UTC (rev 5458)
@@ -143,10 +143,11 @@
 
     abs_velocity_ =
       p->abs_velocity_
-      + cross(p->abs_rot_velocity_, abs_position_)
-      + quatRotate(abs_orientation_, j->getTransVelocity());
+      + cross(p->abs_rot_velocity_, quatRotate(p->abs_orientation_,
+                                               link_->origin_xyz_))
+      + quatRotate(p->abs_orientation_, j->getTransVelocity());
 
-    abs_rot_velocity_ = p->abs_rot_velocity_ + quatRotate(abs_orientation_, j->getRotVelocity());
+    abs_rot_velocity_ = p->abs_rot_velocity_ + quatRotate(p->abs_orientation_, j->getRotVelocity());
 
 
     // Computes the relative frame transform


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