Revision: 7180
http://playerstage.svn.sourceforge.net/playerstage/?rev=7180&view=rev
Author: natepak
Date: 2008-11-27 16:37:06 +0000 (Thu, 27 Nov 2008)
Log Message:
-----------
Added patch 2281269
Modified Paths:
--------------
code/gazebo/trunk/server/physics/Body.cc
code/gazebo/trunk/server/physics/Body.hh
Modified: code/gazebo/trunk/server/physics/Body.cc
===================================================================
--- code/gazebo/trunk/server/physics/Body.cc 2008-11-27 16:30:16 UTC (rev
7179)
+++ code/gazebo/trunk/server/physics/Body.cc 2008-11-27 16:37:06 UTC (rev
7180)
@@ -70,6 +70,9 @@
this->rpyP = new ParamT<Quatern>("rpy", Quatern(), 0);
this->rpyP->Callback( &Body::SetRotation, this );
+
+ this->dampingFactorP = new ParamT<double>("dampingFactor", 0.03, 0);
+
Param::End();
}
@@ -95,6 +98,7 @@
delete this->xyzP;
delete this->rpyP;
+ delete this->dampingFactorP;
}
@@ -107,6 +111,7 @@
this->nameP->Load(node);
this->xyzP->Load(node);
this->rpyP->Load(node);
+ this->dampingFactorP->Load(node);
Pose3d initPose;
initPose.pos = **(this->xyzP);
@@ -217,7 +222,11 @@
{
std::vector< Sensor* >::iterator sensorIter;
std::map< std::string, Geom* >::iterator geomIter;
+ Vector3 vel;
+ Vector3 avel;
+ double force;
+
this->UpdatePose();
if (!this->IsStatic())
@@ -237,6 +246,17 @@
{
(*sensorIter)->Update();
}
+
+ if(this->GetId())
+ {
+ force = this->dampingFactorP->GetValue() * this->mass.mass;
+ vel = this->GetLinearVel();
+ dBodyAddForce(this->GetId(), -((vel.x * fabs(vel.x)) * force),
-((vel.y * fabs(vel.y)) * force), -((vel.z * fabs(vel.z)) * force));
+
+ avel = this->GetAngularVel();
+ dBodyAddTorque(this->GetId(), -avel.x * force, -avel.y * force,
-avel.z * force);
+ }
+
}
////////////////////////////////////////////////////////////////////////////////
@@ -280,7 +300,7 @@
//this->SetPosition(this->staticPose.pos);
//this->SetRotation(this->staticPose.rot);
-
+
for (iter = this->geoms.begin(); iter != this->geoms.end(); iter++)
{
//newPose = (*iter)->GetPose() - this->staticPose;
@@ -702,7 +722,7 @@
{
return dynamic_cast<Model*>(this->GetParent());
}
-
+
////////////////////////////////////////////////////////////////////////////////
/// Get a geom by name
Geom *Body::GetGeom(const std::string &name) const
@@ -719,4 +739,4 @@
return NULL;
}
}
-
+
Modified: code/gazebo/trunk/server/physics/Body.hh
===================================================================
--- code/gazebo/trunk/server/physics/Body.hh 2008-11-27 16:30:16 UTC (rev
7179)
+++ code/gazebo/trunk/server/physics/Body.hh 2008-11-27 16:37:06 UTC (rev
7180)
@@ -1,6 +1,6 @@
/*
* Gazebo - Outdoor Multi-Robot Simulator
- * Copyright (C) 2003
+ * Copyright (C) 2003
* Nate Koenig & Andrew Howard
*
* This program is free software; you can redistribute it and/or modify
@@ -47,97 +47,97 @@
/// \addtogroup gazebo_physics
/// \brief The body class
/// \{
-
+
/// Body class
class Body : public Entity
{
/// \brief Constructor
public: Body(Entity *parent, dWorldID worldId);
-
+
/// \brief Destructor
public: virtual ~Body();
-
+
/// \brief Load the body based on an XMLConfig node
/// \param node XMLConfigNode pointer
public: virtual void Load(XMLConfigNode *node);
-
+
/// \brief Save the body based on our XMLConfig node
public: virtual void Save(std::string &prefix, std::ostream &stream);
-
+
/// \brief Initialize the body
public: virtual void Init();
-
+
/// \brief Finalize the body
public: void Fini();
-
+
/// \brief Update the body
public: virtual void Update();
-
+
/// \brief Attach a geom to this body
/// \param geom Geometery to attach to this body
public: void AttachGeom( Geom *geom );
-
+
/// \brief Set the pose of the body
/// \param pose New pose of the body
public: void SetPose(const Pose3d &pose);
-
+
/// \brief Return the pose of the body
/// \return Pose of the body
public: Pose3d GetPose() const;
-
+
/// \brief Set the position of the body
/// \param pos Vector position
public: void SetPosition(const Vector3 &pos);
-
+
/// \brief Set the rotation of the body
/// \param rot Quaternion rotation
public: void SetRotation(const Quatern &rot);
-
+
/// \brief Return the position of the body
/// \return Position vector
public: Vector3 GetPosition() const;
-
+
/// \brief Return the rotation
/// \return Rotation quaternion
public: Quatern GetRotation() const;
-
+
/// \brief Return the ID of this body
/// \return ODE body id
public: dBodyID GetId() const;
-
+
/// \brief Set whether this body is enabled
public: void SetEnabled(bool enable) const;
-
+
/// \brief Update the center of mass
public: void UpdateCoM();
-
+
/// \brief Get the Center of Mass pose
public: const Pose3d &GetCoMPose() const;
-
+
/// \brief Set whether gravity affects this body
public: void SetGravityMode(bool mode);
-
+
/// \brief Set the linear velocity of the body
public: void SetLinearVel(const Vector3 &vel);
-
+
/// \brief Get the linear velocity of the body
public: Vector3 GetLinearVel() const;
-
+
/// \brief Set the angular velocity of the body
public: void SetAngularVel(const Vector3 &vel);
-
+
/// \brief Get the angular velocity of the body
public: Vector3 GetAngularVel() const;
-
+
/// \brief Set the force applied to the body
public: void SetForce(const Vector3 &force);
-
+
/// \brief Get the force applied to the body
public: Vector3 GetForce() const;
-
+
/// \brief Set the torque applied to the body
public: void SetTorque(const Vector3 &force);
-
+
/// \brief Get the torque applied to the body
public: Vector3 GetTorque() const;
@@ -149,43 +149,48 @@
/// \brief Get the model that this body belongs to
public: Model *GetModel() const;
-
+
+ /// \brief Get the mass of the body
+ public: float GetMass() { return mass.mass; }
+
/// Load a new geom helper function
/// \param node XMLConfigNode used to load the geom
private: void LoadGeom(XMLConfigNode *node);
-
+
/// Load a new sensor
/// \param node XMLConfigNode used to load the geom
private: void LoadSensor(XMLConfigNode *node);
-
+
/// \brief Load a renderable
private: void LoadVisual(XMLConfigNode *node);
/// \brief Update the pose of the body
private: void UpdatePose();
-
+
/// List of geometries attached to this body
private: std::map< std::string, Geom* > geoms;
-
+
/// List of attached sensors
private: std::vector< Sensor* > sensors;
-
+
/// ODE body handle
private: dBodyID bodyId;
-
+
/// Mass properties of the object
private: dMass mass;
-
+
private: bool isStatic;
-
+
private: Pose3d comPose;
private: Pose3d staticPose;
private: Pose3d pose;
-
+
private: ParamT<Vector3> *xyzP;
private: ParamT<Quatern> *rpyP;
+
+ private: ParamT<double> *dampingFactorP;
};
-
+
/// \}
}
This was sent by the SourceForge.net collaborative development platform, the
world's largest Open Source development site.
-------------------------------------------------------------------------
This SF.Net email is sponsored by the Moblin Your Move Developer's challenge
Build the coolest Linux based applications with Moblin SDK & win great prizes
Grand prize is a trip for two to an Open Source event anywhere in the world
http://moblin-contest.org/redirect.php?banner_id=100&url=/
_______________________________________________
Playerstage-commit mailing list
Playerstage-commit@lists.sourceforge.net
https://lists.sourceforge.net/lists/listinfo/playerstage-commit
