Ok, a made a svn diff on my current working directory, I then clean this
up by hand a bit.
I removed some other stuff that is unrelated.
You can find my latest atempt att sending in a patch that adds the
missing imu player interface files here:
http://sourceforge.net/tracker/?func=detail&aid=3195458&group_id=42445&atid=433166
This patch is against a very recent trunk revsion so you will have to
make some modifications. I previously tried to commit this path and
those files should still be in the tracker if you want to look for them.
That version may be more likely to work with gazebo 0.9. But I think the
only difference is that some file changed name, gazebo.h to gz.h, and
maybe some function names were changes.
After taking a look at, or applying that patch you can check the svn
diff file that will hopefully be attached to this email.
Do not try to apply this as a patch, as I have made some manual cleanup.
Also some contents may overlap with the IMU patch.
In the diff file you can find my additional (more or less ugly) changes
that I made to get velocity derivative in the accelerometer output.
If you find something in there that looks suspicious, please let me
know. I have not really made sure that it actually works as it is
suposed to, (it seems to however).
/Peter
On 03/30/2011 04:42 PM, Neostek wrote:
Hi Peter and John,
I'm facing the same problem with getting acceleration from gazebo and i'm
using a flying robot.
Do you think it could be possible to have your code (i mean the imu
interface and the gazebo files you modified)? I need to solve this problem
and a "starting point" would be of great help!
(Obviously i'll share with you my solution when i'll come to one :)
Thanks in advance.
Marco
@John: Thanks a lot for opening thread. It could be very useful!
John Hsu-2 wrote:
Hi,
I've opened a ticket for this issue
https://kforge.ros.org/gazebo/trac/ticket/24
Please feel free to update the ticket.
Thanks,
<https://kforge.ros.org/gazebo/trac/ticket/24>John
On Tue, Mar 29, 2011 at 7:25 AM, Peter Nordin<peter.nor...@liu.se> wrote:
Hello
Unfortunately I do not think that I can help you, but I am having a
similar problem.
I am have been trying to use the IMU sensor, (I have added a few missing
player interface related files), but in my case the acceleration is
allways zero. Anyway I was not able to figure out why so I made some
minor additions to the gazebo code and differentiated the linear
velocity between each timestep. This way the reported acceleration will
be the actual velocity derivative. The problem is that I wont get
g=-9.81 in my output. This is OK in my ground robot case as I usually
just remove the gravity component anyway. For flying robots I do not not
know if this is Ok.
I tried adding an additional "artificial" gravity vector (rotated into
the curent body coordinate system) but realised that when the robot was
moving along the Z-axis (very rarely), I would get double acceleration.
-9.81 as well as the actual velocity derivative (-9.81 when free
falling). I the end I removed the artificial gravity vector, as I actual
have no real use for it.
It is probably possible to solve this in some smart way but I have not
had time to try and figure one out.
I am using a fairly recent Gazebo svn version and player 3.0.2
/Peter
On 03/28/2011 05:26 PM, Neostek wrote:
Hello everyone,
i'm using Gazebo to simulate a PD controller on a quadrotor UAV.
The control evalueates the force and torque to apply to the quadrotor
and
then i use the methods
dBodyAddRelForce(body_id, 0, 0, thrust) and dBodyAddRelTorque(body_id,
roll_torque, pitch_torque, 0).
Now i would like to get information about position, velocity and
acceleration of the quadrotor.
To do so, i use the methods:
body->GetPosition()
body->GetLinearVel()
body->GetLinearAccel()
and the last one appears to give practically nonsense results (gravity
acceleration depending on the mass for instance...).
Browsing into the code i found out that GetLinearAccel() simply does
return this->GetForce() / this->GetMass();
That is " F = m*a " as it should be.
Anyway the controller works and the quadrotor get still on the target
point,
but the acceleration i get on the z axis is positive (e.g. = 19.6) even
though the quadrotor is not varying its height.
I really have no point...please help me! :)
Thanks in advance.
Marco
For the sake of completeness i'm using the following software version:
Gazebo - 0.9.0
Player - 2.1.3
------------------------------------------------------------------------------
Enable your software for Intel(R) Active Management Technology to meet
the
growing manageability and security demands of your customers. Businesses
are taking advantage of Intel(R) vPro (TM) technology - will your
software
be a part of the solution? Download the Intel(R) Manageability Checker
today! http://p.sf.net/sfu/intel-dev2devmar
_______________________________________________
Playerstage-gazebo mailing list
Playerstage-gazebo@lists.sourceforge.net
https://lists.sourceforge.net/lists/listinfo/playerstage-gazebo
------------------------------------------------------------------------------
Enable your software for Intel(R) Active Management Technology to meet the
growing manageability and security demands of your customers. Businesses
are taking advantage of Intel(R) vPro (TM) technology - will your software
be a part of the solution? Download the Intel(R) Manageability Checker
today! http://p.sf.net/sfu/intel-dev2devmar
_______________________________________________
Playerstage-gazebo mailing list
Playerstage-gazebo@lists.sourceforge.net
https://lists.sourceforge.net/lists/listinfo/playerstage-gazebo
Index: libgazebo/gz.h
===================================================================
--- libgazebo/gz.h (revision 8858)
+++ libgazebo/gz.h (working copy)
@@ -1075,6 +1075,8 @@
{
public: GazeboData head;
public: Pose velocity;
+ public: Vec3 acceleration;
+ public: Vec3 eulerAngles;
};
class ImuIface : public Iface
Index: server/sensors/imu/ImuSensor.hh
===================================================================
--- server/sensors/imu/ImuSensor.hh (revision 8858)
+++ server/sensors/imu/ImuSensor.hh (working copy)
@@ -56,10 +56,15 @@
protected: virtual void FiniChild();
public: Pose3d GetVelocity();
+ public: Vector3 GetAcceleration();
+ public: Vector3 GetEulerAngles();
private: Pose3d prevPose;
private: Pose3d imuVel;
-
+ private: Vector3 imuAcc;
+ private: Vector3 eulerAngles;
+ private: Vector3 prevLinVel;
+
};
}
Index: server/sensors/imu/ImuSensor.cc
===================================================================
--- server/sensors/imu/ImuSensor.cc (revision 8858)
+++ server/sensors/imu/ImuSensor.cc (working copy)
@@ -37,6 +37,7 @@
#include "XMLConfig.hh"
#include "Controller.hh"
#include "ImuSensor.hh"
+#include "Simulator.hh"
#include "Vector3.hh"
@@ -96,13 +97,23 @@
return this->imuVel;
}
+Vector3 ImuSensor::GetEulerAngles()
+{
+ return this->eulerAngles;
+}
+
+Vector3 ImuSensor::GetAcceleration()
+{
+ return this->imuAcc;
+}
+
//////////////////////////////////////////////////////////////////////////////
// Update the sensor information
void ImuSensor::UpdateChild()
{
// if (this->active)
{
- Vector3 velocity;
+ Vector3 linVelocity, rotVelocity;
Pose3d poseDelta;
double heading;
double v1;
@@ -117,26 +128,54 @@
poseDelta = this->body->GetWorldPose() - this->prevPose;
this->prevPose = this->body->GetWorldPose();
- velocity = this->body->GetWorldLinearVel();
+ linVelocity = this->body->GetWorldLinearVel();
rot = this->body->GetWorldPose().rot.GetAsEuler();
pose = this->body->GetWorldPose().pos;
- heading = atan2(velocity.y, velocity.x);
+ heading = atan2(linVelocity.y, linVelocity.x);
- v1 = sqrt(pow(velocity.x,2) + pow(velocity.y,2));
+ v1 = sqrt(pow(linVelocity.x,2) + pow(linVelocity.y,2));
vlong = v1 * cos(heading - rot.z);
vlat = v1 * sin(heading - rot.z);
this->imuVel.pos.x = vlong;
this->imuVel.pos.y = vlat;
-
this->imuVel.pos.z = 0;
- velocity = this->body->GetWorldAngularVel();
- this->imuVel.rot.x = velocity.x;
- this->imuVel.rot.y = velocity.y;
- this->imuVel.rot.z = velocity.z;
+ //rotVelocity = this->body->GetWorldAngularVel();
+ rotVelocity = this->body->GetRelativeAngularVel();
+ this->imuVel.rot.x = rotVelocity.x;
+ this->imuVel.rot.y = rotVelocity.y;
+ this->imuVel.rot.z = rotVelocity.z;
+
+ //Asking for acceleration does not seem to work, faking acceleration based
on velocity derivative and gravity vector instead
+ Vector3 velDerivative, gravityVector;
+ //TODO: Lets assume gravity is downwards for now, have not found where to
ask for the actual gravity vector
+ gravityVector.Set(0, 0, -9.81);
+ //Transform gravity to imu coordinate system
+ gravityVector =
this->body->GetWorldPose().rot.RotateVectorReverse(gravityVector);
+ //std::cout << "gravityVector: " << gravityVector << std::endl;
+
+ Time dT = this->simulator->GetSimTime() - this->lastUpdate;
+ //Time dT = this->updatePeriod;
+ //std::cout << "updatePeriod: " << this->updatePeriod << " dT: " << dT <<
" velocity: " << velocity << " prevLinVel: " << prevLinVel << std::endl;
+ //std::cout << "linWorldVel: " << linVelocity << std::endl;
+ //std::cout << "linImuVel: " <<
this->body->GetWorldPose().rot.RotateVectorReverse(linVelocity) << std::endl;
+ velDerivative = (linVelocity - prevLinVel)/dT.Double();
+ this->prevLinVel = linVelocity;
+
+ //transform velDerivative to imu coordinate system
+ velDerivative =
this->body->GetWorldPose().rot.RotateVectorReverse(velDerivative);
+ //std::cout << "velDerivative: " << velDerivative << std::endl;
+
+ this->imuAcc = velDerivative /*+ gravityVector*/;
+ //this->imuAcc = this->body->GetWorldLinearAccel();
+ //std::cout << imuAcc.x << " " << imuAcc.y << " " << imuAcc.z << std::endl;
+
+ //this->imuAcc = linVelocity; //Temporary hack to test reading velicitys
directly
+ this->eulerAngles = rot;
+
}
}
Index: server/controllers/imu/Generic_Imu.cc
===================================================================
--- server/controllers/imu/Generic_Imu.cc (revision 8858)
+++ server/controllers/imu/Generic_Imu.cc (working copy)
@@ -35,6 +35,7 @@
#include "ControllerFactory.hh"
#include "Generic_Imu.hh"
#include "ImuSensor.hh"
+#include "Simulator.hh"
using namespace gazebo;
@@ -108,19 +109,31 @@
{
Pose3d velocity;
+ Vector3 eulerAngles, acceleration;
+
velocity = this->myParent->GetVelocity();
-
+ acceleration = this->myParent->GetAcceleration();
+ eulerAngles = this->myParent->GetEulerAngles();
+
if (this->imuIface->Lock(1))
{
// Data timestamp
- //this->imuIface->data->head.time = Simulator::Instance()->GetSimTime();
+ this->imuIface->data->head.time =
Simulator::Instance()->GetSimTime().Double();
+ this->imuIface->data->eulerAngles.x = eulerAngles[0];
+ this->imuIface->data->eulerAngles.y = eulerAngles[1];
+ this->imuIface->data->eulerAngles.z = eulerAngles[2];
+
this->imuIface->data->velocity.pos.x = velocity.pos.x;
this->imuIface->data->velocity.pos.y = velocity.pos.y;
this->imuIface->data->velocity.pos.z = velocity.pos.z;
this->imuIface->data->velocity.roll = velocity.rot.x;
this->imuIface->data->velocity.pitch = velocity.rot.y;
this->imuIface->data->velocity.yaw = velocity.rot.z;
+
+ this->imuIface->data->acceleration.x = acceleration[0];
+ this->imuIface->data->acceleration.y = acceleration[1];
+ this->imuIface->data->acceleration.z = acceleration[2];
this->imuIface->Unlock();
------------------------------------------------------------------------------
Create and publish websites with WebMatrix
Use the most popular FREE web apps or write code yourself;
WebMatrix provides all the features you need to develop and
publish your website. http://p.sf.net/sfu/ms-webmatrix-sf
_______________________________________________
Playerstage-gazebo mailing list
Playerstage-gazebo@lists.sourceforge.net
https://lists.sourceforge.net/lists/listinfo/playerstage-gazebo
