Revision: 7382
http://playerstage.svn.sourceforge.net/playerstage/?rev=7382&view=rev
Author: natepak
Date: 2009-03-08 03:31:13 +0000 (Sun, 08 Mar 2009)
Log Message:
-----------
Set State velocities and accelerations are in the models local coord frame
Modified Paths:
--------------
code/branches/federation/gazebo/examples/libgazebo/simiface/simiface.cc
code/branches/federation/gazebo/server/Quatern.cc
code/branches/federation/gazebo/server/Quatern.hh
code/branches/federation/gazebo/server/World.cc
code/branches/federation/gazebo/server/physics/ContactParams.cc
code/branches/federation/gazebo/server/physics/ContactParams.hh
code/branches/federation/gazebo/worlds/pioneer2dx.world
Modified:
code/branches/federation/gazebo/examples/libgazebo/simiface/simiface.cc
===================================================================
--- code/branches/federation/gazebo/examples/libgazebo/simiface/simiface.cc
2009-03-08 03:27:54 UTC (rev 7381)
+++ code/branches/federation/gazebo/examples/libgazebo/simiface/simiface.cc
2009-03-08 03:31:13 UTC (rev 7382)
@@ -1,5 +1,6 @@
#include <string.h>
#include <iostream>
+#include <math.h>
#include <gazebo/gazebo.h>
int main()
@@ -40,8 +41,9 @@
{
gazebo::Pose pose;
- pose.pos.x = i+0.1;
+ //pose.pos.x = i+0.1;
pose.pos.z = .145;
+ pose.yaw = M_PI/2;
gazebo::Vec3 linearVel(0.2, 0, 0);
gazebo::Vec3 angularVel(0, 0, 0);
gazebo::Vec3 linearAccel(0, 0, 0);
@@ -49,7 +51,7 @@
simIface->SetState(name, pose, linearVel, angularVel,
linearAccel, angularAccel );
- usleep(900000);
+ usleep(90000000);
}
Modified: code/branches/federation/gazebo/server/Quatern.cc
===================================================================
--- code/branches/federation/gazebo/server/Quatern.cc 2009-03-08 03:27:54 UTC
(rev 7381)
+++ code/branches/federation/gazebo/server/Quatern.cc 2009-03-08 03:31:13 UTC
(rev 7382)
@@ -274,6 +274,28 @@
}
////////////////////////////////////////////////////////////////////////////////
+/// Rotate a vector using the quaternion
+Vector3 Quatern::RotateVector(Vector3 vec) const
+{
+ Quatern tmp;
+ Vector3 result;
+
+ tmp.u = 0.0;
+ tmp.x = vec.x;
+ tmp.y = vec.y;
+ tmp.z = vec.z;
+
+ tmp = (*this) * (tmp * this->GetInverse());
+
+ result.x = tmp.x;
+ result.y = tmp.y;
+ result.z = tmp.z;
+
+ return result;
+}
+
+
+////////////////////////////////////////////////////////////////////////////////
// See if a quatern is finite (e.g., not nan)
bool Quatern::IsFinite() const
{
Modified: code/branches/federation/gazebo/server/Quatern.hh
===================================================================
--- code/branches/federation/gazebo/server/Quatern.hh 2009-03-08 03:27:54 UTC
(rev 7381)
+++ code/branches/federation/gazebo/server/Quatern.hh 2009-03-08 03:31:13 UTC
(rev 7382)
@@ -125,6 +125,10 @@
/// \return This quatern multiplied by the parameter
public: Quatern operator*( const Quatern &qt ) const;
+ /// \brief Rotate a vector using the quaternion
+ /// \return The rotated vector
+ public: Vector3 RotateVector(Vector3 vec) const;
+
/// \brief See if a quatern is finite (e.g., not nan)
/// \return True if quatern is finite
public: bool IsFinite() const;
Modified: code/branches/federation/gazebo/server/World.cc
===================================================================
--- code/branches/federation/gazebo/server/World.cc 2009-03-08 03:27:54 UTC
(rev 7381)
+++ code/branches/federation/gazebo/server/World.cc 2009-03-08 03:31:13 UTC
(rev 7382)
@@ -611,11 +611,18 @@
// The the model's pose
pose.rot.SetFromEuler(
- Vector3(req->modelPose.roll,
+ Vector3(
+ req->modelPose.roll,
req->modelPose.pitch,
req->modelPose.yaw));
model->SetPose(pose);
+ linearVel = pose.rot.RotateVector(linearVel);
+ angularVel = pose.rot.RotateVector(angularVel);
+
+ linearAccel = pose.rot.RotateVector(linearAccel);
+ angularAccel = pose.rot.RotateVector(angularAccel);
+
// Set the model's linear and angular velocity
model->SetLinearVel(linearVel);
model->SetAngularVel(angularVel);
Modified: code/branches/federation/gazebo/server/physics/ContactParams.cc
===================================================================
--- code/branches/federation/gazebo/server/physics/ContactParams.cc
2009-03-08 03:27:54 UTC (rev 7381)
+++ code/branches/federation/gazebo/server/physics/ContactParams.cc
2009-03-08 03:31:13 UTC (rev 7382)
@@ -45,6 +45,8 @@
this->mu2 = dInfinity;
this->slip1 = 0.1;
this->slip2 = 0.1;
+
+ this->enabled = true;
}
//////////////////////////////////////////////////////////////////////////////
Modified: code/branches/federation/gazebo/server/physics/ContactParams.hh
===================================================================
--- code/branches/federation/gazebo/server/physics/ContactParams.hh
2009-03-08 03:27:54 UTC (rev 7381)
+++ code/branches/federation/gazebo/server/physics/ContactParams.hh
2009-03-08 03:31:13 UTC (rev 7382)
@@ -78,6 +78,8 @@
/// \brief soft constraint force mixing
public: double softCfm;
+
+ public: bool enabled;
public: boost::signal< void (Geom*, Geom*)> contactSignal;
};
Modified: code/branches/federation/gazebo/worlds/pioneer2dx.world
===================================================================
--- code/branches/federation/gazebo/worlds/pioneer2dx.world 2009-03-08
03:27:54 UTC (rev 7381)
+++ code/branches/federation/gazebo/worlds/pioneer2dx.world 2009-03-08
03:31:13 UTC (rev 7382)
@@ -95,7 +95,7 @@
<model:physical name="pioneer2dx_model1">
<xyz>0 0 .145</xyz>
- <rpy>0.0 0.0 0.0</rpy>
+ <rpy>0.0 0.0 90.0</rpy>
<enableGravity>false</enableGravity>
<enableFriction>false</enableFriction>
This was sent by the SourceForge.net collaborative development platform, the
world's largest Open Source development site.
------------------------------------------------------------------------------
Open Source Business Conference (OSBC), March 24-25, 2009, San Francisco, CA
-OSBC tackles the biggest issue in open source: Open Sourcing the Enterprise
-Strategies to boost innovation and cut costs with open source participation
-Receive a $600 discount off the registration fee with the source code: SFAD
http://p.sf.net/sfu/XcvMzF8H
_______________________________________________
Playerstage-commit mailing list
[email protected]
https://lists.sourceforge.net/lists/listinfo/playerstage-commit
