Permalink
Browse files

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

  • Loading branch information...
1 parent 0e620eb commit 314e392c98f204d151cee326c85422bdc2e533d8 rtv committed Jan 19, 2010
View
@@ -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"
View
@@ -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 sina = sin(desired_pose.a);
+ double cosa = cos(DesiredPose.a);
+ double sina = sin(DesiredPose.a);
- desired_pose.x += (axis.x * cosa - axis.y * sina) * start_position;
- desired_pose.y += (axis.x * sina + axis.y * cosa) * start_position;
- desired_pose.z += axis.z * start_position;
- SetPose( desired_pose );
+ DesiredPose.x += (axis.x * cosa - axis.y * sina) * start_position;
+ DesiredPose.y += (axis.x * sina + axis.y * cosa) * start_position;
+ DesiredPose.z += axis.z * start_position;
+ SetPose( DesiredPose );
} break;
case TYPE_ROTATIONAL:
{
- desired_pose.a += start_position;
- SetPose( desired_pose);
+ DesiredPose.a += start_position;
+ 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:
{
@@ -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$
//
///////////////////////////////////////////////////////////////////////////
@@ -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$
//
///////////////////////////////////////////////////////////////////////////
@@ -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$
//
///////////////////////////////////////////////////////////////////////////
@@ -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$
//
///////////////////////////////////////////////////////////////////////////
View
@@ -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$
//
///////////////////////////////////////////////////////////////////////////
@@ -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$
//
///////////////////////////////////////////////////////////////////////////
View
@@ -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$
//
///////////////////////////////////////////////////////////////////////////
View
@@ -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$
//
///////////////////////////////////////////////////////////////////////////
View
@@ -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:
View
@@ -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;
View
@@ -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"
@@ -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>
View
@@ -1,3 +1,8 @@
+
+/**
+ $Id$
+ **/
+
/** @defgroup world World
Stage simulates a 'world' composed of `models', defined in a `world
View
@@ -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>
View
@@ -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
View
@@ -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

0 comments on commit 314e392

Please sign in to comment.