Revision: 7011 http://playerstage.svn.sourceforge.net/playerstage/?rev=7011&view=rev Author: natepak Date: 2008-09-09 14:25:39 +0000 (Tue, 09 Sep 2008)
Log Message: ----------- Applied patch 1905901 Modified Paths: -------------- code/gazebo/trunk/libgazebo/gazebo.h code/gazebo/trunk/server/controllers/position2d/SConscript Added Paths: ----------- code/gazebo/trunk/server/controllers/position2d/holonome3sw/ code/gazebo/trunk/server/controllers/position2d/holonome3sw/Holonome3Sw_Position2d.cc code/gazebo/trunk/server/controllers/position2d/holonome3sw/Holonome3Sw_Position2d.hh code/gazebo/trunk/server/controllers/position2d/holonome3sw/SConscript code/gazebo/trunk/worlds/models/wizbot.model code/gazebo/trunk/worlds/wizbot.world Modified: code/gazebo/trunk/libgazebo/gazebo.h =================================================================== --- code/gazebo/trunk/libgazebo/gazebo.h 2008-09-09 14:06:20 UTC (rev 7010) +++ code/gazebo/trunk/libgazebo/gazebo.h 2008-09-09 14:25:39 UTC (rev 7011) @@ -346,8 +346,6 @@ /// Type of model that owns this interface public: std::string modelType; - - }; /** \} */ Modified: code/gazebo/trunk/server/controllers/position2d/SConscript =================================================================== --- code/gazebo/trunk/server/controllers/position2d/SConscript 2008-09-09 14:06:20 UTC (rev 7010) +++ code/gazebo/trunk/server/controllers/position2d/SConscript 2008-09-09 14:25:39 UTC (rev 7011) @@ -1,12 +1,7 @@ #Import variable Import('env sharedObjs') -dirs = Split('differential steering') +dirs = Split('differential steering holonome3sw') for subdir in dirs : SConscript('%s/SConscript' % subdir) - -#sources = Split('') - -#staticObjs.append(env.StaticObject(sources)) -#sharedObjs.append(env.SharedObject(sources)) Added: code/gazebo/trunk/server/controllers/position2d/holonome3sw/Holonome3Sw_Position2d.cc =================================================================== --- code/gazebo/trunk/server/controllers/position2d/holonome3sw/Holonome3Sw_Position2d.cc (rev 0) +++ code/gazebo/trunk/server/controllers/position2d/holonome3sw/Holonome3Sw_Position2d.cc 2008-09-09 14:25:39 UTC (rev 7011) @@ -0,0 +1,269 @@ +/* + * Gazebo - Outdoor Multi-Robot Simulator + * Copyright (C) 2003 + * Nate Koenig & Andrew Howard + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + * + */ +/* + * Desc: Position2d controller for a Holonome3Sw drive. + * Author: Christian Gagneraud (ch'Gans), based on Differential_Position2d.cc + * Date: 21 Feb 2007 + * SVN info: $Id$ + */ + +#include "XMLConfig.hh" +#include "Model.hh" +#include "Global.hh" +#include "HingeJoint.hh" +#include "World.hh" +#include "Simulator.hh" +#include "gazebo.h" +#include "GazeboError.hh" +#include "ControllerFactory.hh" +#include "Holonome3Sw_Position2d.hh" + +using namespace gazebo; + +GZ_REGISTER_STATIC_CONTROLLER("holonome3sw_position2d", Holonome3Sw_Position2d); + +enum {RIGHT, LEFT}; + +//////////////////////////////////////////////////////////////////////////////// +// Constructor +Holonome3Sw_Position2d::Holonome3Sw_Position2d(Entity *parent ) + : Controller(parent) +{ + this->myParent = dynamic_cast<Model*>(this->parent); + + if (!this->myParent) + gzthrow("Holonome3Sxw_Position2d controller requires a Model as its parent"); + + ResetData(); +} + +//////////////////////////////////////////////////////////////////////////////// +// Destructor +Holonome3Sw_Position2d::~Holonome3Sw_Position2d() +{ +} + +//////////////////////////////////////////////////////////////////////////////// +// Load the controller +void Holonome3Sw_Position2d::LoadChild(XMLConfigNode *node) +{ + this->myIface = dynamic_cast<PositionIface*>(this->ifaces[0]); + + if (!this->myIface) + gzthrow("Holonome3Sw_Position2d controller requires a PositionIface"); + + // Get wheels child + node = node->GetChild("wheels"); + if (!node) + gzthrow("Holonome3Sw_Position2d controller requires a <wheels> node"); + + // Get default params + float distdef = node->GetFloat("distance", 0.0, 0); + float radiusdef = node->GetFloat("radius", 0.0, 0); + float maxtorquedef = node->GetFloat("torque", 0.0, 0); + float betadef = node->GetFloat("beta", 0.0, 0); + float gammadef = node->GetFloat("gamma", 0.0, 0); + + // Get params for each wheels + for (size_t i = 0; i < 3; ++i) + { + std::ostringstream sw; sw << "swedish" << i; + XMLConfigNode *lnode = node->GetChild(sw.str()); + if (!lnode) + gzthrow("The controller couldn't get swedish wheels " + sw.str()); + + std::string joint = lnode->GetString("joint", "", 1); + this->joint[i] = dynamic_cast<HingeJoint*>(this->myParent->GetJoint(joint)); + if (!this->joint[i]) + gzthrow("The controller couldn't get hinge joint for " + sw.str()); + + this->ALPHA[i] = lnode->GetFloat("alpha", 1000000, 1); + if (this->ALPHA[i] == 1000000) + gzthrow("The controller could't get alpha param for " + sw.str()); + + this->DIST[i] = lnode->GetFloat("distance", distdef, 0); + if (!this->DIST[i]) + gzthrow("The controller could't get distance param for " + sw.str()); + + this->BETA[i] = lnode->GetFloat("beta", betadef, 0); + + this->GAMMA[i] = lnode->GetFloat("gamma", gammadef, 0); + + this->RADIUS[i] = lnode->GetFloat("radius", radiusdef, 0); + if (!this->RADIUS[i]) + gzthrow("The controller could't get radius param for " + sw.str()); + + this->MAXTORQUE[i] = lnode->GetFloat("torque", maxtorquedef, 0); + if (!this->MAXTORQUE[i]) + gzthrow("The controller could't get torque param for " + sw.str()); + + // Pre-compute some constants + A[i] = (2*3.14159265358979)*fmodf(ALPHA[i]+BETA[i]+GAMMA[i], 360)/360; + L[i] = DIST[i]*cos((2*3.14159265358979)*fmodf(BETA[i]+GAMMA[i], 360)/360); + R[i] = RADIUS[i]*cos((2*3.14159265358979)*fmodf(GAMMA[i], 360)/360); + } + +#if 0 + std::cout << "Holonomous robot, here are the params:" << std::endl; + for (size_t i = 0; i < 3; ++i) + { + std::cout << " - swedish" << i << std::endl; + std::cout << " alpha = " << ALPHA[i] << std::endl; + std::cout << " beta = " << BETA[i] << std::endl; + std::cout << " gamma = " << GAMMA[i] << std::endl; + std::cout << " dist = " << DIST[i] << std::endl; + std::cout << " radius = " << RADIUS[i] << std::endl; + std::cout << " max-torque = " << MAXTORQUE[i] << std::endl; + std::cout << " A = " << A[i] << std::endl; + std::cout << " L = " << L[i] << std::endl; + std::cout << " R = " << R[i] << std::endl; + std::cout << std::endl; + } +#endif +} + +//////////////////////////////////////////////////////////////////////////////// +// Load the controller +/*void Holonome3Sw_Position2d::SaveChild(XMLConfigNode *node) +{ + node = node->GetChild("wheels"); + if (!node) + gzthrow("No such child node: wheels"); + for (size_t i = 0; i < 3; ++i) + { + std::ostringstream sw; sw << "swedish" << i; + XMLConfigNode *lnode = node->GetChild(sw.str()); + if (!lnode) + gzthrow("No such child node wheels/" + sw.str()); + lnode->SetValue("alpha", this->ALPHA[i]); + lnode->SetValue("distance", this->DIST[i]); + lnode->SetValue("beta", this->BETA[i]); + lnode->SetValue("gamma", this->GAMMA[i]); + lnode->SetValue("radius", this->RADIUS[i]); + lnode->SetValue("max-torque", this->MAXTORQUE[i]); + // "joint" ??? + } +}*/ + +//////////////////////////////////////////////////////////////////////////////// +// Initialize the controller +void Holonome3Sw_Position2d::InitChild() +{ + ResetData(); +} + +void Holonome3Sw_Position2d::ResetChild() +{ + ResetData(); +} + +//////////////////////////////////////////////////////////////////////////////// +// Update the controller +void Holonome3Sw_Position2d::UpdateChild() +{ + this->GetPositionCmd(); + + //std::cout << "Anchors: {"; + for (size_t i = 0; i < 3; ++i) + { + //std::cout << "[" << this->joint[i]->GetAnchor() << "] "; + if (this->enableMotors) + { + this->joint[i]->SetParam( dParamVel, this->PhiP[i]); + this->joint[i]->SetParam( dParamFMax, this->MAXTORQUE[i] ); + } + else + { + this->joint[i]->SetParam( dParamVel, 0 ); + this->joint[i]->SetParam( dParamVel, 0 ); + } + } + //std::cout << "}" << std::endl; + this->PutPositionData(); +} + +//////////////////////////////////////////////////////////////////////////////// +// Finalize the controller +void Holonome3Sw_Position2d::FiniChild() +{ +} + +//////////////////////////////////////////////////////////////////////////////// +// Reset internal data to 0 +void Holonome3Sw_Position2d::ResetData() +{ + for (size_t i = 0; i < 3; ++i) + { + Xi[i] = 0; + XiP[i] = 0; + PhiP[i] = 0; + } + this->enableMotors = true; +} + +////////////////////////////////////////////////////////////////////////////// +// Get commands from the external interface +void Holonome3Sw_Position2d::GetPositionCmd() +{ + if (this->myIface->Lock(1)) + { + double vx = this->myIface->data->cmdVelocity.pos.x; + double vy = this->myIface->data->cmdVelocity.pos.y; + double va = this->myIface->data->cmdVelocity.yaw; + for (size_t i = 0; i < 3; ++i) + { + this->PhiP[i] = -(1/R[i])*(-sin(A[i])*vx + + cos(A[i])*vy + + L[i]*va); + } + this->enableMotors = this->myIface->data->cmdEnableMotors > 0; + this->myIface->Unlock(); +#if 0 + std::cout << "Xi=[" << Xi[0] << ", " << Xi[1] << ", " << Xi[2] << "]; "; + std::cout << "vx=" << vx <<", vy=" << vy << ", va=" << va << "; "; + std::cout << "PhiP=[" << PhiP[0] << ", " << PhiP[1] << ", " << PhiP[2] << "];\n"; +#endif + } +} + +////////////////////////////////////////////////////////////////////////////// +// Update the data in the interface +void Holonome3Sw_Position2d::PutPositionData() +{ + if (this->myIface->Lock(1)) + { + // TODO: Data timestamp + this->myIface->data->head.time = Simulator::Instance()->GetSimTime(); + + this->myIface->data->pose.pos.x = this->Xi[0]; + this->myIface->data->pose.pos.y = this->Xi[1]; + this->myIface->data->pose.yaw = NORMALIZE(this->Xi[2]); + + this->myIface->data->velocity.pos.x = 0; + this->myIface->data->velocity.pos.y = 0; + this->myIface->data->velocity.yaw = 0; + + // TODO + this->myIface->data->stall = 0; + + this->myIface->Unlock(); + } +} Added: code/gazebo/trunk/server/controllers/position2d/holonome3sw/Holonome3Sw_Position2d.hh =================================================================== --- code/gazebo/trunk/server/controllers/position2d/holonome3sw/Holonome3Sw_Position2d.hh (rev 0) +++ code/gazebo/trunk/server/controllers/position2d/holonome3sw/Holonome3Sw_Position2d.hh 2008-09-09 14:25:39 UTC (rev 7011) @@ -0,0 +1,211 @@ +/* + * Gazebo - Outdoor Multi-Robot Simulator + * Copyright (C) 2003 + * Nate Koenig & Andrew Howard + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + * + */ + +/* + * Desc: Position2d controller for a Holonome3Sw drive. + * Author: Christian Gagneraud (ch'Gans), based on Differential_Position2d.hh + * Date: 21 Feb 2007 + * SVN: $Id$ + */ + +#ifndef HOLONOME3SW_POSITION2D_HH +#define HOLONOME3SW_POSITION2D_HH + +#include <map> + +#include "Controller.hh" + +namespace gazebo +{ + class HingeJoint; + class Entity; + class PositionIface; + +/// \addtogroup gazebo_controller +/// \{ +/** \defgroup holonome3sw_position2d holonome3sw_position2d + + \brief Position2D controller for an holonomous robot using 3 swedish + wheels. + + This is a controller that simulates the motion of an holonomous + robot using 3 swedish wheels (such as WizBot). + + Kinematics model: + \verbatim + J1.R(Th).XiP + J2.PhiP = 0 + + J1 = [ -sin(A1) cos(A1) L1 ] + [ -sin(A2) cos(A2) L2 ] + [ -sin(A3) cos(A3) L3 ] + + J2 = [ R1 0 0 ] + [ 0 R2 0 ] + [ 0 0 R3 ] + + R(t) = [ cos(t) sin(t) 0 ] + [ -sin(t) cos(t) 0 ] + [ 0 0 1 ] + + With: + A1 = alpha1+beta1+gamma1; + L1 = l1*cos(beta1+gamma1) + R1 = r1*cos(gamma1) + \endverbatim + + \verbatim + <controller:holonome3sw_position2d name="controller-name"> + <wheels> + <-- Global to all wheels, optional --> + <distance>d</distance> + <-- Global to all wheels, optional --> + <radius>r</radius> + <-- Global to all wheels, optional --> + <max-torque>t</max-torque> + <-- Global to all wheels, optional --> + <beta>b</beta> + <-- Global to all wheels, optional --> + <gamma>g</gamma> + <-- 1st wheel --> + <swedish0> + <joint-name>joint-name</joint-name> + <alpha>a</alpha> + <-- default to wheels/distance --> + <distance>d</distance> + <-- default to wheels/radius --> + <radius>r</radius> + <-- default to wheels/max-torque --> + <max-torque>max-torque</max-torque> + <-- default to wheels/beta if exist or to zero --> + <beta>b</beta> + <-- default to wheels/beta if exist or to zero --> + <gamma>g</gamma> + </swedish0> + <-- 2nd wheel --> + <swedish1> + ... + </swedish> + <-- 3rd wheel --> + <swedish2> + ... + </swedish> + </wheels> + <interface:position name="iface-name"/> + </controller:holonome3sw_position2d> + \endverbatim + + Where: + - P: Reference point on the robot frame. + - A: Point representing the center of a wheel + - distance, alpha: Polar coordinates of A in the robot frame. + - beta: Orientation of the plane of the wheel with respect with (PA). + - gamma: Direction, with respect to the wheel plane, of the zero + component of the velocity of the contact point. + + \{ +*/ + +/// \brief WizBot Position2D controller. +/// This is a controller that simulates a WizBot motion +class Holonome3Sw_Position2d : public Controller +{ + /// Constructor + public: Holonome3Sw_Position2d(Entity *parent ); + + /// Destructor + public: virtual ~Holonome3Sw_Position2d(); + + /// Load the controller + /// \param node XML config node + protected: virtual void LoadChild(XMLConfigNode *node); + + /// Init the controller + protected: virtual void InitChild(); + + /// \brief Reset the controller + protected: void ResetChild(); + + /// Update the controller + protected: virtual void UpdateChild(); + + /// Finalize the controller + protected: virtual void FiniChild(); + + + /// Update the data in the interface + private: void PutPositionData(); + + /// Get the position command from libgazebo + private: void GetPositionCmd(); + + /// Reset internal datas + private: void ResetData(); + + /// The Position interface + private: PositionIface *myIface; + + /// The parent Model + private: Model *myParent; + + + // The wheel joints. + private: HingeJoint *joint[3]; + + /// Polar coordinate of the center of the wheels + private: float DIST[3]; + + /// Polar coordinate of the center of the wheels + private: float ALPHA[3]; + + /// Orientation of the plane of the wheel + private: float BETA[3]; + + /// Direction of the zero component of the velocity of the contact point. + private: float GAMMA[3]; + + /// Radius of the wheels + private: float RADIUS[3]; + + /// Max torque to applied to the wheels + private: float MAXTORQUE[3]; + + // Robot posture coordinates in the world: x, y, theta + private: float Xi[3]; + // Robot speed coordinates in the world: vx, vy, vtheta + private: float XiP[3]; + // Rotation speed coordinates (wheels, rad/s) + private: float PhiP[3]; + + /// True = enable motors + private: bool enableMotors; + +private: + float A[3], L[3], R[3]; + +}; + +/** \} */ +/// \} + +} + +#endif + Added: code/gazebo/trunk/server/controllers/position2d/holonome3sw/SConscript =================================================================== --- code/gazebo/trunk/server/controllers/position2d/holonome3sw/SConscript (rev 0) +++ code/gazebo/trunk/server/controllers/position2d/holonome3sw/SConscript 2008-09-09 14:25:39 UTC (rev 7011) @@ -0,0 +1,6 @@ +#Import variable +Import('env sharedObjs') + +sources = Split('Holonome3Sw_Position2d.cc') + +sharedObjs.append(env.SharedObject(sources)) Added: code/gazebo/trunk/worlds/models/wizbot.model =================================================================== --- code/gazebo/trunk/worlds/models/wizbot.model (rev 0) +++ code/gazebo/trunk/worlds/models/wizbot.model 2008-09-09 14:25:39 UTC (rev 7011) @@ -0,0 +1,234 @@ +<?xml version="1.0"?> + +<!-- Generic WizBot Model --> +<model:physical + name="default_wizbot_model" + xmlns:xi="http://www.w3.org/2001/XInclude" + xmlns:model="http://playerstage.sourceforge.net/gazebo/xmlschema/#model" + xmlns:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor" + xmlns:body="http://playerstage.sourceforge.net/gazebo/xmlschema/#body" + xmlns:geom="http://playerstage.sourceforge.net/gazebo/xmlschema/#geom" + xmlns:joint="http://playerstage.sourceforge.net/gazebo/xmlschema/#joint" + > + + <xyz>0 0 0.0</xyz> + <rpy>0.0 0.0 0.0</rpy> + + <canonicalBody>chassis_body</canonicalBody> + + <body:cylinder name="chassis_body"> + <xyz>0.0 0.0 0.0</xyz> + <rpy>0.0 0.0 0.0</rpy> + + <geom:cylinder name="chassis_geom"> + <xyz>0 0 0</xyz> + <size>0.140 0.2</size> + <mass>2</mass> + <visual> + <size>0.280 0.280 0.2</size> + <mesh>unit_cylinder</mesh> + <material>Gazebo/RustyBarrel</material> + </visual> + </geom:cylinder> + </body:cylinder> + + <body:cylinder name="wheel0"> + <xyz>0.175 0 -0.0725</xyz> + <rpy>0 90 0</rpy> + + <geom:cylinder name="wheel0_geom"> + <size>0.075 0.05</size> + <mass>0.237</mass> + + <visual> + <rpy>-90 0 0</rpy> + <size>0.125 0.05 0.125</size> + <mesh>Pioneer2dx/tire.mesh</mesh> + <material>Gazebo/Black</material> + </visual> + + <visual> + <rpy>-90 0 0</rpy> + <size>0.073 0.05 0.073</size> + <mesh>Pioneer2at/wheel.mesh</mesh> + <material>Gazebo/Gold</material> + </visual> + + <visual> + <rpy>0 0 0</rpy> + <xyz>0 0 -0.015</xyz> + <size>0.04 0.04 0.08</size> + <mesh>unit_cylinder</mesh> + <material>Gazebo/Black</material> + </visual> + + <surface> + <type>0</type> + <mu1>1.7</mu1> + <bounce>0.1</bounce> + <bounceVel>0.04</bounceVel> + </surface> + + <surface> + <type>1</type> + <mu1>1.7</mu1> + <bounce>0.1</bounce> + <bounceVel>0.04</bounceVel> + </surface> + + <surface> + <type>2</type> + <mu1>0.01</mu1> + <mu2>1.7</mu2> + <mu1Dir>0 0 1</mu1Dir> + <bounce>0.1</bounce> + <bounceVel>0.04</bounceVel> + </surface> + + </geom:cylinder> + </body:cylinder> + + <body:cylinder name="wheel1"> + <xyz>-0.0875 0.151554445662 -0.0725</xyz> + <rpy>0 90 120</rpy> + + <geom:cylinder name="wheel1_geom"> + <size>0.075 0.05</size> + <mass>0.237</mass> + + <visual> + <rpy>-90 0 0</rpy> + <size>0.125 0.05 0.125</size> + <mesh>Pioneer2dx/tire.mesh</mesh> + <material>Gazebo/Black</material> + </visual> + + <visual> + <rpy>-90 0 0</rpy> + <size>0.073 0.05 0.073 </size> + <mesh>Pioneer2at/wheel.mesh</mesh> + <material>Gazebo/Gold</material> + </visual> + + <visual> + <rpy>0 0 0</rpy> + <xyz>0 0 -0.015</xyz> + <size>0.04 0.04 0.08 </size> + <mesh>unit_cylinder</mesh> + <material>Gazebo/Black</material> + </visual> + + <surface> + <type>0</type> + <mu1>1.7</mu1> + <bounce>0.1</bounce> + <bounceVel>0.04</bounceVel> + </surface> + + <surface> + <type>1</type> + <mu1>1.7</mu1> + <bounce>0.1</bounce> + <bounceVel>0.04</bounceVel> + </surface> + + <surface> + <type>2</type> + <mu1>0.01</mu1> + <mu2>1.7</mu2> + <mu1Dir>0 0 1</mu1Dir> + <bounce>0.1</bounce> + <bounceVel>0.04</bounceVel> + </surface> + + </geom:cylinder> + </body:cylinder> + + <body:cylinder name="wheel2"> + <xyz>-0.0875 -0.151554445662 -0.0725</xyz> + <rpy>0 90 240</rpy> + + <geom:cylinder name="wheel2_geom"> + <size>0.075 0.05</size> + <mass>0.237</mass> + + <visual> + <rpy>-90 0 0</rpy> + <size>0.125 0.05 0.125</size> + <mesh>Pioneer2dx/tire.mesh</mesh> + <material>Gazebo/Black</material> + </visual> + + <visual> + <rpy>-90 0 0</rpy> + <size>0.073 0.05 0.073 </size> + <mesh>Pioneer2at/wheel.mesh</mesh> + <material>Gazebo/Gold</material> + </visual> + + <visual> + <rpy>0 0 0</rpy> + <xyz>0 0 -0.015</xyz> + <size>0.04 0.04 0.08 </size> + <mesh>unit_cylinder</mesh> + <material>Gazebo/Black</material> + </visual> + + <surface> + <type>0</type> + <mu1>1.7</mu1> + <bounce>0.1</bounce> + <bounceVel>0.04</bounceVel> + </surface> + + <surface> + <type>1</type> + <mu1>1.7</mu1> + <bounce>0.1</bounce> + <bounceVel>0.04</bounceVel> + </surface> + + <surface> + <type>2</type> + <mu1>0.01</mu1> + <mu2>1.7</mu2> + <mu1Dir>0 0 1</mu1Dir> + <bounce>0.1</bounce> + <bounceVel>0.04</bounceVel> + </surface> + + </geom:cylinder> + </body:cylinder> + + + <joint:hinge name="wheel0_hinge"> + <body1>wheel0</body1> + <body2>chassis_body</body2> + <anchor>wheel0</anchor> + <anchorOffset>-0.04 0 0</anchorOffset> + <axis>1 0 0 </axis> + <erp>0.8</erp> + <cfm>10e-5</cfm> + </joint:hinge> + + <joint:hinge name="wheel1_hinge"> + <body1>wheel1</body1> + <body2>chassis_body</body2> + <anchor>wheel1</anchor> + <anchorOffset>0.02 -0.034641016 0</anchorOffset> + <axis>-0.5 0.866025404 0</axis> + <erp>0.8</erp> + <cfm>10e-5</cfm> + </joint:hinge> + + <joint:hinge name="wheel2_hinge"> + <body1>wheel2</body1> + <body2>chassis_body</body2> + <anchor>wheel2</anchor> + <anchorOffset>0.02 0.034641016 0</anchorOffset> + <axis>-0.5 -0.866025404 0</axis> + <erp>0.8</erp> + <cfm>10e-5</cfm> + </joint:hinge> + +</model:physical> Added: code/gazebo/trunk/worlds/wizbot.world =================================================================== --- code/gazebo/trunk/worlds/wizbot.world (rev 0) +++ code/gazebo/trunk/worlds/wizbot.world 2008-09-09 14:25:39 UTC (rev 7011) @@ -0,0 +1,175 @@ +<?xml version="1.0"?> + +<gazebo:world + xmlns:xi="http://www.w3.org/2001/XInclude" + xmlns:gazebo="http://playerstage.sourceforge.net/gazebo/xmlschema/#gz" + xmlns:model="http://playerstage.sourceforge.net/gazebo/xmlschema/#model" + xmlns:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor" + xmlns:window="http://playerstage.sourceforge.net/gazebo/xmlschema/#window" + xmlns:param="http://playerstage.sourceforge.net/gazebo/xmlschema/#param" + xmlns:body="http://playerstage.sourceforge.net/gazebo/xmlschema/#body" + xmlns:geom="http://playerstage.sourceforge.net/gazebo/xmlschema/#geom" + xmlns:joint="http://playerstage.sourceforge.net/gazebo/xmlschema/#joint" + xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface" + xmlns:ui="http://playerstage.sourceforge.net/gazebo/xmlschema/#ui" + xmlns:rendering="http://playerstage.sourceforge.net/gazebo/xmlschema/#rendering" + xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller" + xmlns:physics="http://playerstage.sourceforge.net/gazebo/xmlschema/#physics" > + + <verbosity>5</verbosity> + + <physics:ode> + <stepTime>0.03</stepTime> + <gravity>0 0 -9.81</gravity> + <cfm>0.008</cfm> + <erp>0.2</erp> + </physics:ode> + + <rendering:gui> + <type>fltk</type> + <size>800 600</size> + <pos>0 0</pos> + </rendering:gui> + + <rendering:ogre> + <ambient>1.0 1.0 1.0 1.0</ambient> + <sky> + <material>Gazebo/CloudySky</material> + </sky> + + </rendering:ogre> + + <!-- Ground Plane --> + <model:physical name="plane1_model"> + <xyz>0 0 0</xyz> + <rpy>0 0 0</rpy> + <static>true</static> + + <body:plane name="plane1_body"> + <geom:plane name="plane1_geom"> + <normal>0 0 1</normal> + <size>2000 2000</size> + <segments>10 10</segments> + <uvTile>100 100</uvTile> + <material>Gazebo/GrassFloor</material> + <surface> + <mu1>10</mu1> + </surface> + </geom:plane> + </body:plane> + </model:physical> + + <!-- Main Camera --> + <model:physical name="cam1_model"> + <xyz>-2 0 2</xyz> + <rpy>0 25 0</rpy> + <static>true</static> + <body:empty name="cam1_body"> + <sensor:camera name="cam1_sensor"> + <imageSize>640 480</imageSize> + <hfov>60</hfov> + <nearClip>0.1</nearClip> + <farClip>100</farClip> + <saveFrames>false</saveFrames> + <saveFramePath>frames</saveFramePath> + <controller:generic_camera name="controller1"> + <interface:camera name="camera_iface_0"/> + </controller:generic_camera> + </sensor:camera> + </body:empty> + </model:physical> + + <model:physical name="cylinder1_model"> + <xyz>1 1 0.5</xyz> + <rpy>0 0 0</rpy> + <body:cylinder name="cylinder1_body"> + <geom:cylinder name="cylinder1_geom"> + <size>0.5 1</size> + <mass>1</mass> + <visual> + <mesh>unit_cylinder</mesh> + <material>Gazebo/RustyBarrel</material> + </visual> + </geom:cylinder> + </body:cylinder> + </model:physical> + + + <model:physical name="pioneer2dx_model1"> + <xyz>1 0 0.145</xyz> + <rpy>0.0 0.0 0.0</rpy> + + <controller:differential_position2d name="controller1"> + <leftJoint>left_wheel_hinge</leftJoint> + <rightJoint>right_wheel_hinge</rightJoint> + <wheelSeparation>0.34</wheelSeparation> + <wheelDiameter>0.15</wheelDiameter> + <torque>5</torque> + <interface:position name="position_iface_1"/> + </controller:differential_position2d> + + <model:physical name="laser"> + <xyz>0.15 0 0.18</xyz> + + <attach> + <parentBody>chassis_body</parentBody> + <myBody>laser_body</myBody> + </attach> + + <include embedded="true"> + <xi:include href="models/sicklms200.model" /> + </include> + </model:physical> + + <include embedded="true"> + <xi:include href="models/pioneer2dx.model" /> + </include> + </model:physical> + + <model:physical name="wizbot_model1"> + <xyz>0 0 0.1</xyz> + <rpy>0 0 0</rpy> + + <controller:holonome3sw_position2d name="controller1"> + + <wheels> + <radius>0.075</radius> + <distance>0.175</distance> + <torque>2</torque> + <swedish0> + <joint>wheel0_hinge</joint> + <alpha>0</alpha> + </swedish0> + <swedish1> + <joint>wheel1_hinge</joint> + <alpha>120</alpha> + </swedish1> + <swedish2> + <joint>wheel2_hinge</joint> + <alpha>240</alpha> + </swedish2> + </wheels> + + <interface:position name="position_iface_0"/> + + </controller:holonome3sw_position2d> + + <include embedded="true"> + <xi:include href="models/wizbot.model" /> + </include> + + </model:physical> + + + <!-- White Directional light --> + <model:renderable name="directional_white"> + <light> + <type>directional</type> + <direction>0 -0.6 -0.4</direction> + <diffuseColor>1.0 1.0 1.0</diffuseColor> + <specularColor>0.2 0.2 0.2</specularColor> + <attenuation>1000 1.0 0.0 0</attenuation> + </light> + </model:renderable> + +</gazebo:world> 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