Menu
▾
▴
[Playerstage-commit] SF.net SVN: playerstage:[7010] code/gazebo/trunk
|
From: <na...@us...> - 2008-09-09 07:06:13
|
Revision: 7010
http://playerstage.svn.sourceforge.net/playerstage/?rev=7010&view=rev
Author: natepak
Date: 2008-09-09 14:06:20 +0000 (Tue, 09 Sep 2008)
Log Message:
-----------
Applied patch 1998580 and 1998581
Modified Paths:
--------------
code/gazebo/trunk/libgazebo/Iface.cc
code/gazebo/trunk/libgazebo/gazebo.h
code/gazebo/trunk/player/GazeboDriver.cc
code/gazebo/trunk/player/SConscript
code/gazebo/trunk/server/controllers/SConscript
code/gazebo/trunk/server/physics/Joint.cc
code/gazebo/trunk/server/physics/Joint.hh
Added Paths:
-----------
code/gazebo/trunk/player/OpaqueInterface.cc
code/gazebo/trunk/player/OpaqueInterface.hh
code/gazebo/trunk/server/controllers/opaque/
code/gazebo/trunk/server/controllers/opaque/SConscript
code/gazebo/trunk/server/controllers/opaque/jointforce/
code/gazebo/trunk/server/controllers/opaque/jointforce/JointForce.cc
code/gazebo/trunk/server/controllers/opaque/jointforce/JointForce.hh
code/gazebo/trunk/server/controllers/opaque/jointforce/SConscript
Modified: code/gazebo/trunk/libgazebo/Iface.cc
===================================================================
--- code/gazebo/trunk/libgazebo/Iface.cc 2008-09-09 13:44:47 UTC (rev 7009)
+++ code/gazebo/trunk/libgazebo/Iface.cc 2008-09-09 14:06:20 UTC (rev 7010)
@@ -57,6 +57,7 @@
GZ_REGISTER_IFACE("actarray", ActarrayIface);
GZ_REGISTER_IFACE("ptz", PTZIface);
GZ_REGISTER_IFACE("stereocamera", StereoCameraIface);
+GZ_REGISTER_IFACE("opaque", OpaqueIface);
//////////////////////////////////////////////////////////////////////////////
// Create an interface
Modified: code/gazebo/trunk/libgazebo/gazebo.h
===================================================================
--- code/gazebo/trunk/libgazebo/gazebo.h 2008-09-09 13:44:47 UTC (rev 7009)
+++ code/gazebo/trunk/libgazebo/gazebo.h 2008-09-09 14:06:20 UTC (rev 7010)
@@ -1413,6 +1413,63 @@
/** \} */
/// \}
+/***************************************************************************/
+/// \addtogroup libgazebo_iface
+/// \{
+/** \defgroup opaque_iface opaque
+
+ \brief Interface for arbitrary data transfer
+The opaque interface can transmit any data
+
+\{
+ */
+/// Maximum amount of data we will be sending. 8MB is the maximum dictated by Player
+#define GAZEBO_MAX_OPAQUE_DATA 1024*1024*8
+/// \brief opaque data
+class OpaqueData
+{
+ public: GazeboData head;
+
+ /// The length of the data (in bytes)
+ public: uint32_t data_count;
+
+ /// The data we will be sending
+ public: uint8_t data[GAZEBO_MAX_OPAQUE_DATA];
+};
+
+
+/// \brief Opaque interface
+class OpaqueIface : public Iface
+{
+ /// \brief Constructor
+ public: OpaqueIface():Iface("opaque", sizeof(OpaqueIface)+sizeof(OpaqueData)) {}
+
+ /// \brief Destructor
+ public: virtual ~OpaqueIface() {this->data = NULL;}
+
+ /// \brief Create the server
+ public: virtual void Create(Server *server, std::string id)
+ {
+ Iface::Create(server,id);
+ this->data = (OpaqueData*)this->mMap;
+ }
+
+ /// \brief Open the iface
+ public: virtual void Open(Client *client, std::string id)
+ {
+ Iface::Open(client,id);
+ this->data = (OpaqueData*)this->mMap;
+ }
+
+ /// Pointer to the opaque data
+ public: OpaqueData *data;
+};
+
+/** \} */
+/// \}
+
+
+
}
Modified: code/gazebo/trunk/player/GazeboDriver.cc
===================================================================
--- code/gazebo/trunk/player/GazeboDriver.cc 2008-09-09 13:44:47 UTC (rev 7009)
+++ code/gazebo/trunk/player/GazeboDriver.cc 2008-09-09 14:06:20 UTC (rev 7010)
@@ -37,6 +37,7 @@
#include "FiducialInterface.hh"
#include "Position3dInterface.hh"
#include "ActarrayInterface.hh"
+#include "OpaqueInterface.hh"
#include "PTZInterface.hh"
#include "GripperInterface.hh"
@@ -295,6 +296,11 @@
ifsrc = new ActarrayInterface( playerAddr, this, cf, section );
break;
+ case PLAYER_OPAQUE_CODE:
+ if (!player_quiet_startup) printf(" an opaque interface.\n");
+ ifsrc = new OpaqueInterface( playerAddr, this, cf, section );
+ break;
+
case PLAYER_PTZ_CODE:
if (!player_quiet_startup) printf(" a ptz interface.\n");
ifsrc = new PTZInterface( playerAddr, this, cf, section );
@@ -314,13 +320,7 @@
if (!player_quiet_startup) printf(" a sonar interface.\n");
ifsrc = new SonarInterface( playerAddr, this, cf, section );
break;
- case PLAYER_TRUTH_CODE:
- if (!player_quiet_startup) printf(" a truth interface.\n");
- ifsrc = new TruthInterface( playerAddr, this, cf, section );
- break;
-
-
case PLAYER_GPS_CODE:
if (!player_quiet_startup) printf(" a gps interface.\n");
ifsrc = new GpsInterface( playerAddr, this, cf, section );
Added: code/gazebo/trunk/player/OpaqueInterface.cc
===================================================================
--- code/gazebo/trunk/player/OpaqueInterface.cc (rev 0)
+++ code/gazebo/trunk/player/OpaqueInterface.cc 2008-09-09 14:06:20 UTC (rev 7010)
@@ -0,0 +1,146 @@
+/*
+ * 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: Opaque Interface for Player
+ * Author: Benjamin Kloster
+ * Date: 13 March 2008
+ */
+
+/**
+@addtogroup player
+@par Opaque Interface
+*/
+/* TODO
+Do we need these?
+- PLAYER_OPAQUE_DATA_STATE
+- PLAYER_OPAQUE_CMD_DATA
+- PLAYER_OPAQUE_REQ_DATA
+*/
+#include <math.h>
+
+#include "GazeboError.hh"
+#include "gazebo.h"
+#include "GazeboDriver.hh"
+#include "OpaqueInterface.hh"
+
+using namespace gazebo;
+
+///////////////////////////////////////////////////////////////////////////////
+// Constructor
+OpaqueInterface::OpaqueInterface(player_devaddr_t addr,
+ GazeboDriver *driver, ConfigFile *cf, int section)
+ : GazeboInterface(addr, driver, cf, section)
+{
+ // Get the ID of the interface
+ this->gz_id = (char*) calloc(1024, sizeof(char));
+ strcat(this->gz_id, GazeboClient::prefixId);
+ strcat(this->gz_id, cf->ReadString(section, "gz_id", ""));
+
+ // Allocate a Position Interface
+ this->iface = new OpaqueIface();
+
+ this->datatime = -1;
+}
+
+///////////////////////////////////////////////////////////////////////////////
+// Destructor
+OpaqueInterface::~OpaqueInterface()
+{
+ // Release this interface
+ delete this->iface;
+}
+
+///////////////////////////////////////////////////////////////////////////////
+// Handle all messages. This is called from GazeboDriver
+int OpaqueInterface::ProcessMessage(QueuePointer &respQueue,
+ player_msghdr_t *hdr, void *data)
+{
+ if (this->iface->Lock(1))
+ {
+ // nothing yet
+ return 0;
+ }
+ else
+ this->Unsubscribe();
+
+ return -1;
+}
+
+///////////////////////////////////////////////////////////////////////////////
+// Update this interface, publish new info. This is
+// called from GazeboDriver::Update
+void OpaqueInterface::Update()
+{
+ player_opaque_data_t data;
+ struct timeval ts;
+
+ memset(&data, 0, sizeof(data));
+ if (this->iface->Lock(1))
+ {
+ // Only Update when new data is present
+ if (this->iface->data->head.time > this->datatime)
+ {
+ this->datatime = this->iface->data->head.time;
+
+ ts.tv_sec = (int) (this->iface->data->head.time);
+ ts.tv_usec = (int) (fmod(this->iface->data->head.time, 1) * 1e6);
+
+ data.data_count = this->iface->data->data_count;
+ data.data = this->iface->data->data;
+
+ this->driver->Publish( this->device_addr,
+ PLAYER_MSGTYPE_DATA,
+ PLAYER_OPAQUE_DATA_STATE,
+ (void*)&data, sizeof(data), &this->datatime );
+ }
+
+ this->iface->Unlock();
+ }
+ else
+ this->Unsubscribe();
+}
+
+
+///////////////////////////////////////////////////////////////////////////////
+// Open a SHM interface when a subscription is received. This is called from
+// GazeboDriver::Subscribe
+void OpaqueInterface::Subscribe()
+{
+ // Open the interface
+ try
+ {
+ this->iface->Open(GazeboClient::client, this->gz_id);
+ }
+ catch (std::string e)
+ {
+ //std::ostringstream stream;
+ std::cout <<"Error Subscribing to Gazebo Opaque Interface\n"
+ << e << "\n";
+ //gzthrow(stream.str());
+ exit(0);
+ }
+}
+
+///////////////////////////////////////////////////////////////////////////////
+// Close a SHM interface. This is called from GazeboDriver::Unsubscribe
+void OpaqueInterface::Unsubscribe()
+{
+ this->iface->Close();
+}
Added: code/gazebo/trunk/player/OpaqueInterface.hh
===================================================================
--- code/gazebo/trunk/player/OpaqueInterface.hh (rev 0)
+++ code/gazebo/trunk/player/OpaqueInterface.hh 2008-09-09 14:06:20 UTC (rev 7010)
@@ -0,0 +1,83 @@
+/*
+ * 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: Opaque Interface for Player
+ * Author: Benjamin Kloster
+ * Date: 13 March 2008
+ */
+
+#ifndef OPAQUEINTERFACE_HH
+#define OPAQUEINTERFACE_HH
+
+#include "GazeboInterface.hh"
+
+namespace gazebo
+{
+
+/// \addtogroup player_iface
+/// \{
+/// \defgroup opaque_player Opaque Interface
+/// \brief Opaque Player interface
+/// \{
+
+// Forward declarations
+class OpaqueIface;
+
+/// \brief Opaque Player interface
+class OpaqueInterface : public GazeboInterface
+{
+ /// \brief Constructor
+ public: OpaqueInterface(player_devaddr_t addr, GazeboDriver *driver,
+ ConfigFile *cf, int section);
+
+ /// \brief Destructor
+ public: virtual ~OpaqueInterface();
+
+ /// \brief Handle all messages. This is called from GazeboDriver
+ public: virtual int ProcessMessage(QueuePointer &respQueue,
+ player_msghdr_t *hdr, void *data);
+
+ /// \brief Update this interface, publish new info.
+ public: virtual void Update();
+
+ /// \brief Open a SHM interface when a subscription is received.
+ /// This is called fromGazeboDriver::Subscribe
+ public: virtual void Subscribe();
+
+ /// \brief Close a SHM interface. This is called from
+ /// GazeboDriver::Unsubscribe
+ public: virtual void Unsubscribe();
+
+ private: OpaqueIface *iface;
+
+ /// \brief Gazebo id. This needs to match and ID in a Gazebo WorldFile
+ private: char *gz_id;
+
+ /// \brief Timestamp on last data update
+ private: double datatime;
+};
+
+/// \}
+/// \}
+
+
+}
+
+#endif
Modified: code/gazebo/trunk/player/SConscript
===================================================================
--- code/gazebo/trunk/player/SConscript 2008-09-09 13:44:47 UTC (rev 7009)
+++ code/gazebo/trunk/player/SConscript 2008-09-09 14:06:20 UTC (rev 7010)
@@ -14,6 +14,7 @@
'CameraInterface.cc',
'FiducialInterface.cc',
'PTZInterface.cc',
+ 'OpaqueInterface.cc',
'ActarrayInterface.cc',
'GripperInterface.cc'
]
Modified: code/gazebo/trunk/server/controllers/SConscript
===================================================================
--- code/gazebo/trunk/server/controllers/SConscript 2008-09-09 13:44:47 UTC (rev 7009)
+++ code/gazebo/trunk/server/controllers/SConscript 2008-09-09 14:06:20 UTC (rev 7010)
@@ -1,7 +1,7 @@
#Import variable
Import('env sharedObjs headers')
-dirs = Split('position2d laser camera factory gripper actarray ptz')
+dirs = Split('position2d laser camera factory gripper actarray ptz opaque')
if env['with_audio'] == 'yes':
dirs+=Split('audio')
Added: code/gazebo/trunk/server/controllers/opaque/SConscript
===================================================================
--- code/gazebo/trunk/server/controllers/opaque/SConscript (rev 0)
+++ code/gazebo/trunk/server/controllers/opaque/SConscript 2008-09-09 14:06:20 UTC (rev 7010)
@@ -0,0 +1,7 @@
+#Import variable
+Import('env sharedObjs')
+
+dirs = Split('jointforce contact')
+
+for subdir in dirs :
+ SConscript('%s/SConscript' % subdir)
Added: code/gazebo/trunk/server/controllers/opaque/jointforce/JointForce.cc
===================================================================
--- code/gazebo/trunk/server/controllers/opaque/jointforce/JointForce.cc (rev 0)
+++ code/gazebo/trunk/server/controllers/opaque/jointforce/JointForce.cc 2008-09-09 14:06:20 UTC (rev 7010)
@@ -0,0 +1,121 @@
+/*
+ * 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: Joint Force controller
+ * Author: Benjamin Kloster
+ * Date: 13 March 2008
+ */
+
+#include "Global.hh"
+#include "XMLConfig.hh"
+#include "Model.hh"
+#include "World.hh"
+#include "gazebo.h"
+#include "GazeboError.hh"
+#include "ControllerFactory.hh"
+#include "Simulator.hh"
+#include "JointForce.hh"
+
+using namespace gazebo;
+
+GZ_REGISTER_STATIC_CONTROLLER("jointforce", JointForce);
+
+////////////////////////////////////////////////////////////////////////////////
+// Constructor
+JointForce::JointForce(Entity *parent )
+ : Controller(parent)
+{
+ this->myParent = dynamic_cast<Model*>(this->parent);
+
+ if (!this->myParent)
+ gzthrow("ControllerStub controller requires a Model as its parent");
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// Destructor
+JointForce::~JointForce()
+{
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// Load the controller
+void JointForce::LoadChild(XMLConfigNode *node)
+{
+ XMLConfigNode *jNode;
+ Joint *joint;
+ std::string jointName;
+ dJointFeedback *jFeedback = new dJointFeedback;
+ int i =0;
+ this->myIface = dynamic_cast<OpaqueIface*>(this->ifaces[0]);
+ if (!this->myIface) {
+ gzthrow("JointForce controller requires an OpaqueIface");
+ }
+ jNode = node->GetChild("joint");
+ while(jNode && i < GAZEBO_JOINTFORCE_CONTROLLER_MAX_FEEDBACKS)
+ {
+ jointName = jNode->GetString("name","",1);
+ joint = this->myParent->GetJoint(jointName);
+ jFeedback = joint->GetFeedback();
+ if(jFeedback) {
+ this->jointfeedbacks[i] = jFeedback;
+ i++;
+ }
+ jNode = jNode->GetNext("joint");
+ }
+ this->n_joints = i;
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// Initialize the controller
+void JointForce::InitChild()
+{
+ //this->myIface->data->data = new uint8_t[sizeof(dJointFeedback)*n_joints];
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// Update the controller
+void JointForce::UpdateChild()
+{
+ this->myIface->Lock(1);
+ this->myIface->data->head.time = Simulator::Instance()->GetSimTime();
+ // Let me explain this number: each joint reports 4 vectors: Force and torque
+ // on each jointed object, respectively. These vectors have 4 elements: x,y,z
+ // and a fourth one. So we transmit 4 dReals per vector.
+ this->myIface->data->data_count = n_joints*4*4*sizeof(dReal);
+
+ for(int i=0; i< n_joints; i++) {
+ // Copy vector for force on first object
+ memcpy(this->myIface->data->data + (i*4 + 0)*4*sizeof(dReal), this->jointfeedbacks[i]->f1, 4*sizeof(dReal));
+ // Copy vector for torque on first object
+ memcpy(this->myIface->data->data + (i*4 + 1)*4*sizeof(dReal), this->jointfeedbacks[i]->t1, 4*sizeof(dReal));
+ // Copy vector for force on second object
+ memcpy(this->myIface->data->data + (i*4 + 2)*4*sizeof(dReal), this->jointfeedbacks[i]->f2, 4*sizeof(dReal));
+ // Copy vector for torque on second object
+ memcpy(this->myIface->data->data + (i*4 + 3)*4*sizeof(dReal), this->jointfeedbacks[i]->t2, 4*sizeof(dReal));
+ }
+ this->myIface->Unlock();
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// Finalize the controller
+void JointForce::FiniChild()
+{
+}
Added: code/gazebo/trunk/server/controllers/opaque/jointforce/JointForce.hh
===================================================================
--- code/gazebo/trunk/server/controllers/opaque/jointforce/JointForce.hh (rev 0)
+++ code/gazebo/trunk/server/controllers/opaque/jointforce/JointForce.hh 2008-09-09 14:06:20 UTC (rev 7010)
@@ -0,0 +1,92 @@
+/*
+ * 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: Joint Force Controller
+ * Author: Benjamin Kloster
+ * Date: 13 March 2008
+ */
+#ifndef JOINTFORCE_CONTROLLER_HH
+#define JOINTFORCE_CONTROLLER_HH
+
+/// Maximum number of joints that can be watched by one controller
+#define GAZEBO_JOINTFORCE_CONTROLLER_MAX_FEEDBACKS 16
+
+#include "Controller.hh"
+#include "Entity.hh"
+#include <ode/ode.h>
+#include <sys/time.h>
+
+
+namespace gazebo
+{
+/// \addtogroup gazebo_controller
+/// \{
+/** \defgroup jointforce_controller jointforce
+
+ \brief A controller that measures forces and torques exerted by joints
+
+ \{
+*/
+
+/// \brief A JointForce controller
+class JointForce : public Controller
+{
+ /// Constructor
+ public: JointForce(Entity *parent );
+
+ /// Destructor
+ public: virtual ~JointForce();
+
+ /// Load the controller
+ /// \param node XML config node
+ /// \return 0 on success
+ protected: virtual void LoadChild(XMLConfigNode *node);
+
+ /// Init the controller
+ /// \return 0 on success
+ protected: virtual void InitChild();
+
+ /// Update the controller
+ /// \return 0 on success
+ protected: virtual void UpdateChild();
+
+ /// Finalize the controller
+ /// \return 0 on success
+ protected: virtual void FiniChild();
+
+ /// The parent Model
+ private: Model *myParent;
+
+ /// The Iface. The dJointFeedback structs are rather arbitrary, so we use an Opaque Interface
+ private: OpaqueIface *myIface;
+ /// The joint feedbacks
+ private: dJointFeedback *jointfeedbacks[GAZEBO_JOINTFORCE_CONTROLLER_MAX_FEEDBACKS];
+ /// The number of joints we are watching
+ private: int n_joints;
+};
+
+/** \} */
+/// \}
+
+}
+
+#endif
+
Added: code/gazebo/trunk/server/controllers/opaque/jointforce/SConscript
===================================================================
--- code/gazebo/trunk/server/controllers/opaque/jointforce/SConscript (rev 0)
+++ code/gazebo/trunk/server/controllers/opaque/jointforce/SConscript 2008-09-09 14:06:20 UTC (rev 7010)
@@ -0,0 +1,7 @@
+#Import variable
+Import('env sharedObjs')
+
+sources = Split('JointForce.cc')
+
+#staticObjs.append(env.StaticObject(sources))
+sharedObjs.append(env.SharedObject(sources))
Modified: code/gazebo/trunk/server/physics/Joint.cc
===================================================================
--- code/gazebo/trunk/server/physics/Joint.cc 2008-09-09 13:44:47 UTC (rev 7009)
+++ code/gazebo/trunk/server/physics/Joint.cc 2008-09-09 14:06:20 UTC (rev 7010)
@@ -49,6 +49,7 @@
this->body2NameP = new ParamT<std::string>("body2",std::string(),1);
this->anchorBodyNameP = new ParamT<std::string>("anchor",std::string(),0);
this->anchorOffsetP = new ParamT<Vector3>("anchorOffset",Vector3(0,0,0), 0);
+ this->provideFeedbackP = new ParamT<bool>("provideFeedback", false, 0);
Param::End();
}
@@ -66,6 +67,7 @@
delete this->body2NameP;
delete this->anchorBodyNameP;
delete this->anchorOffsetP;
+ delete this->provideFeedbackP;
}
@@ -90,6 +92,7 @@
this->erpP->Load(node);
this->cfmP->Load(node);
this->suspensionCfmP->Load(node);
+ this->provideFeedbackP->Load(node);
Body *body1 = this->model->GetBody( **(this->body1NameP));
Body *body2 = this->model->GetBody(**(this->body2NameP));
@@ -129,6 +132,12 @@
this->line2->AddPoint(Vector3(0,0,0));
this->line2->AddPoint(Vector3(0,0,0));
+ if (**this->provideFeedbackP)
+ {
+ this->feedback = new dJointFeedback;
+ dJointSetFeedback(this->jointId, this->feedback);
+ }
+
this->LoadChild(node);
// Set the anchor vector
@@ -337,6 +346,13 @@
}
////////////////////////////////////////////////////////////////////////////////
+/// Get the feedback data structure for this joint, if set
+dJointFeedback *Joint::GetFeedback()
+{
+ return dJointGetFeedback(this->jointId);
+}
+
+////////////////////////////////////////////////////////////////////////////////
/// Get the high stop of an axis(index).
double Joint::GetHighStop(int index)
{
Modified: code/gazebo/trunk/server/physics/Joint.hh
===================================================================
--- code/gazebo/trunk/server/physics/Joint.hh 2008-09-09 13:44:47 UTC (rev 7009)
+++ code/gazebo/trunk/server/physics/Joint.hh 2008-09-09 14:06:20 UTC (rev 7010)
@@ -131,6 +131,9 @@
/// \brief Get the CFM of this joint
public: double GetCFM();
+ /// \brief Get the feedback data structure for this joint, if set
+ public: dJointFeedback *GetFeedback();
+
/// \brief Get the high stop of an axis(index).
public: double GetHighStop(int index=0);
@@ -158,7 +161,11 @@
private: ParamT<std::string> *body2NameP;
private: ParamT<std::string> *anchorBodyNameP;
private: ParamT<Vector3> *anchorOffsetP;
+ private: ParamT<bool> *provideFeedbackP;
+ /// Feedback data for this joint
+ private: dJointFeedback *feedback;
+
protected: std::vector<Param*> parameters;
private: OgreVisual *visual;
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|