## [Playerstage-commit] SF.net SVN: playerstage:[7382] code/branches/federation/gazebo

 [Playerstage-commit] SF.net SVN: playerstage:[7382] code/branches/federation/gazebo From: - 2009-03-08 03:31:20 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 #include +#include #include 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 @@ 0 0 .145 - 0.0 0.0 0.0 + 0.0 0.0 90.0 false false This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.