fixed rotation of model_actuator, thanks to patch #2927791 from RAZOR

rtv · Jan 19, 2010 · 314e392 · 314e392
1 parent 0e620eb
commit 314e392
Show file tree

Hide file tree

Showing 18 changed files with 36 additions and 28 deletions.
diff --git a/libstage/canvas.cc b/libstage/canvas.cc
@@ -7,7 +7,7 @@
     Alex Couture-Beil (asc17@sfu.ca)
     Jeremy Asher (jra11@sfu.ca)
 
-    $Id$
+    $Id: canvas.cc 8330 2009-10-27 19:13:58Z rtv $
 */
 
 #include "stage.hh"

diff --git a/libstage/model_actuator.cc b/libstage/model_actuator.cc
@@ -89,8 +89,7 @@ ModelActuator::~ModelActuator( void )
 void ModelActuator::Load( void )
 {
   Model::Load();
-  InitialPose = GetPose();
+
-
   // load steering mode
   if( wf->PropertyExists( wf_entity, "type" ) )
 	 {
@@ -151,25 +150,25 @@ void ModelActuator::Load( void )
 	{
 		start_position = wf->ReadFloat ( wf_entity, "start_position", 0 );
 
-		Pose desired_pose = InitialPose;
+		Pose DesiredPose = InitialPose;
 
 		switch (actuator_type)
 		  {
 		  case TYPE_LINEAR:
 			 {				 
-				double cosa = cos(desired_pose.a);
+				double cosa = cos(DesiredPose.a);
-				double sina = sin(desired_pose.a);
+				double sina = sin(DesiredPose.a);
 
-				desired_pose.x += (axis.x * cosa - axis.y * sina) * start_position;
+				DesiredPose.x += (axis.x * cosa - axis.y * sina) * start_position;
-				desired_pose.y += (axis.x * sina + axis.y * cosa) * start_position;
+				DesiredPose.y += (axis.x * sina + axis.y * cosa) * start_position;
-				desired_pose.z += axis.z * start_position;
+				DesiredPose.z += axis.z * start_position;
-				SetPose( desired_pose );
+				SetPose( DesiredPose );
 			 } break;
 
 		  case TYPE_ROTATIONAL:
 			 {
-				desired_pose.a += start_position;
+				DesiredPose.a += start_position;
-				SetPose( desired_pose);
+				SetPose( DesiredPose);
 			 }break;
 			default:
 			  PRINT_ERR1( "unrecognized actuator type %d", actuator_type );
@@ -197,7 +196,9 @@ void ModelActuator::Update( void  )
 	{
 		case TYPE_LINEAR:
 		{
-			pos = PoseDiff.x * axis.x + PoseDiff.y * axis.y + PoseDiff.z * axis.z; // Dot product to find distance along axis
+			// When the velocity is applied, it will automatically be rotated by the angle the model is at
+			// So, rotate the axis of movement by the model angle before doing a dot product to find the actuator position
+			pos = (PoseDiff.x * cosa - PoseDiff.y * sina)*axis.x + (PoseDiff.x * sina+ PoseDiff.y * cosa)* axis.y + PoseDiff.z * axis.z;
 		} break;
 		case TYPE_ROTATIONAL:
 		{

diff --git a/libstage/model_blinkenlight.cc b/libstage/model_blinkenlight.cc
@@ -7,7 +7,7 @@
 // CVS info:
 //  $Source: /home/tcollett/stagecvs/playerstage-cvs/code/stage/libstage/model_blinkenlight.cc,v $
 //  $Author: rtv $
-//  $Revision: 1.1 $
+//  $Revision$
 //
 ///////////////////////////////////////////////////////////////////////////
 

diff --git a/libstage/model_blobfinder.cc b/libstage/model_blobfinder.cc
@@ -7,7 +7,7 @@
 // CVS info:
 //  $Source: /home/tcollett/stagecvs/playerstage-cvs/code/stage/libstage/model_blobfinder.cc,v $
 //  $Author: rtv $
-//  $Revision: 1.3 $
+//  $Revision$
 //
 ///////////////////////////////////////////////////////////////////////////
 

diff --git a/libstage/model_fiducial.cc b/libstage/model_fiducial.cc
@@ -7,7 +7,7 @@
 // CVS info:
 //  $Source: /home/tcollett/stagecvs/playerstage-cvs/code/stage/libstage/model_fiducial.cc,v $
 //  $Author: rtv $
-//  $Revision: 1.4 $
+//  $Revision$
 //
 ///////////////////////////////////////////////////////////////////////////
 

diff --git a/libstage/model_gripper.cc b/libstage/model_gripper.cc
@@ -8,7 +8,7 @@
 // CVS info:
 //  $Source: /home/tcollett/stagecvs/playerstage-cvs/code/stage/src/model_gripper.c,v $
 //  $Author: thjc $
-//  $Revision: 1.26.8.1 $
+//  $Revision$
 //
 ///////////////////////////////////////////////////////////////////////////
 

diff --git a/libstage/model_laser.cc b/libstage/model_laser.cc
@@ -5,7 +5,7 @@
 // CVS info:
 //  $Source: /home/tcollett/stagecvs/playerstage-cvs/code/stage/libstage/model_laser.cc,v $
 //  $Author: rtv $
-//  $Revision: 1.7 $
+//  $Revision$
 //
 ///////////////////////////////////////////////////////////////////////////
 

diff --git a/libstage/model_position.cc b/libstage/model_position.cc
@@ -7,7 +7,7 @@
 // CVS info:
 //  $Source: /home/tcollett/stagecvs/playerstage-cvs/code/stage/libstage/model_position.cc,v $
 //  $Author: rtv $
-//  $Revision: 1.5 $
+//  $Revision$
 //
 ///////////////////////////////////////////////////////////////////////////
 

diff --git a/libstage/model_ranger.cc b/libstage/model_ranger.cc
@@ -7,7 +7,7 @@
 // CVS info:
 //  $Source: /home/tcollett/stagecvs/playerstage-cvs/code/stage/libstage/model_ranger.cc,v $
 //  $Author: rtv $
-//  $Revision: 1.4 $
+//  $Revision$
 //
 ///////////////////////////////////////////////////////////////////////////
 

diff --git a/libstage/model_wifi.cc b/libstage/model_wifi.cc
@@ -7,7 +7,7 @@
 // CVS info:
 //  $Source: /home/tcollett/stagecvs/playerstage-cvs/code/stage/libstage/model_wifi.cc,v $
 //  $Author: rtv $
-//  $Revision: 1.2 $
+//  $Revision$
 //
 ///////////////////////////////////////////////////////////////////////////
 

diff --git a/libstage/option.hh b/libstage/option.hh
@@ -14,7 +14,7 @@ namespace Stg {
 	 Used to pass settings between the GUI and the drawing classes
 	 
 	 Author: Jeremy Asher, Richard Vaughan
-	 SVN: $Id$
+	 SVN: $Id: option.hh 8061 2009-07-21 01:49:26Z rtv $
 	 */	
 	class Option {
 	private:

diff --git a/libstage/stage.hh b/libstage/stage.hh
@@ -28,7 +28,7 @@
  *  Desc: External header file for the Stage library
  *  Author: Richard Vaughan (vaughan@sfu.ca) 
  *  Date: 1 June 2003
- *  SVN: $Id$
+ *  SVN: $Id: stage.hh 8516 2010-01-19 22:57:20Z rtv $
  */
 
 // C libs
@@ -2930,6 +2930,8 @@ namespace Stg
 	 double min_position;
 	 double max_position;
 	 double start_position;
+	 double cosa;
+	 double sina;
 	 ControlMode control_mode;
 	 ActuatorType actuator_type;
 	 stg_point3_t axis;

diff --git a/libstage/stagecpp.cc b/libstage/stagecpp.cc
@@ -1,6 +1,6 @@
 // Thin-as-possible C Wrappers for C++ worldfile calls, using a single static worldfile.
 // This is a hacky use of the old C++ worldfile code.
-// $Id$
+// $Id: stagecpp.cc 7151 2008-11-15 03:11:20Z rtv $
 
 #include "stage_internal.h"
 #include "gui.h"

diff --git a/libstage/texture_manager.cc b/libstage/texture_manager.cc
@@ -3,7 +3,7 @@
  *  Stage
  *
  *  Created by Alex Couture-Beil on 03/06/08.
- *  $Id$
+ *  $Id: texture_manager.cc 8070 2009-07-22 06:41:10Z rtv $
  */
 
 #include <sstream>

diff --git a/libstage/world.cc b/libstage/world.cc
@@ -1,3 +1,8 @@
+
+/**
+	$Id$ 
+ **/
+
 /** @defgroup world World
 
     Stage simulates a 'world' composed of `models', defined in a `world

diff --git a/libstage/worldfile.cc b/libstage/worldfile.cc
@@ -24,7 +24,7 @@
  *          Douglas S. Blank <dblank@brynmawr.edu>
  *
  * Date: 15 Nov 2001
- * CVS info: $Id$
+ * CVS info: $Id: worldfile.cc 8313 2009-10-20 16:55:31Z rtv $
  */
 
 #include <assert.h>

diff --git a/libstage/worldfile.hh b/libstage/worldfile.hh
@@ -21,7 +21,7 @@
  * Desc: A class for reading in the world file.
  * Author: Andrew Howard, Richard Vaughan
  * Date: 15 Nov 2001
- * CVS info: $Id$
+ * CVS info: $Id: worldfile.hh 8264 2009-09-26 01:28:12Z rtv $
  */
 
 #ifndef WORLDFILE_HH

diff --git a/libstage/worldgui.cc b/libstage/worldgui.cc
@@ -3,7 +3,7 @@
     Authors: Richard Vaughan (vaughan@sfu.ca)
              Alex Couture-Beil (asc17@sfu.ca)
              Jeremy Asher (jra11@sfu.ca)
-    SVN: $Id$
+    SVN: $Id: worldgui.cc 8329 2009-10-27 06:30:39Z rtv $
 */
 
 /** @defgroup worldgui World with Graphical User Interface