Menu
▾
▴
personalrobots-commit
|
From: <is...@us...> - 2008-07-24 00:00:33
|
Revision: 2016
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=2016&view=rev
Author: isucan
Date: 2008-07-24 00:00:40 +0000 (Thu, 24 Jul 2008)
Log Message:
-----------
added clear option for collision spaces
Modified Paths:
--------------
pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp
pkg/trunk/util/collision_space/include/collision_space/environment.h
pkg/trunk/util/collision_space/include/collision_space/environmentODE.h
pkg/trunk/util/collision_space/src/collision_space/environmentODE.cpp
Modified: pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp 2008-07-23 23:53:12 UTC (rev 2015)
+++ pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp 2008-07-24 00:00:40 UTC (rev 2016)
@@ -148,7 +148,8 @@
data[i3 + 1] = m_cloud.pts[i].y;
data[i3 + 2] = m_cloud.pts[i].z;
}
-
+
+ m_collisionSpace->clearObstacles();
m_collisionSpace->addPointCloud(n, data, 0.01);
delete[] data;
Modified: pkg/trunk/util/collision_space/include/collision_space/environment.h
===================================================================
--- pkg/trunk/util/collision_space/include/collision_space/environment.h 2008-07-23 23:53:12 UTC (rev 2015)
+++ pkg/trunk/util/collision_space/include/collision_space/environment.h 2008-07-24 00:00:40 UTC (rev 2016)
@@ -62,6 +62,9 @@
/** Check if a model is in collision */
virtual bool isCollision(unsigned int model_id) = 0;
+ /** Remove all obstacles from collision model */
+ virtual void clearObstacles(void) = 0;
+
/** Add a point cloud to the collision space */
virtual void addPointCloud(unsigned int n, const double* points, double radius = 0.01) = 0;
Modified: pkg/trunk/util/collision_space/include/collision_space/environmentODE.h
===================================================================
--- pkg/trunk/util/collision_space/include/collision_space/environmentODE.h 2008-07-23 23:53:12 UTC (rev 2015)
+++ pkg/trunk/util/collision_space/include/collision_space/environmentODE.h 2008-07-24 00:00:40 UTC (rev 2016)
@@ -56,11 +56,7 @@
~EnvironmentModelODE(void)
{
- for (unsigned int i = 0 ; i < m_kgeoms.size() ; ++i)
- for (unsigned int j = 0 ; j < m_kgeoms[i].size() ; ++j)
- delete m_kgeoms[i][j];
- if (m_space)
- dSpaceDestroy(m_space);
+ freeMemory();
}
dSpaceID getODESpace(void) const;
@@ -68,6 +64,9 @@
/** Check if a model is in collision */
virtual bool isCollision(unsigned int model_id);
+ /** Remove all obstacles from collision model */
+ virtual void clearObstacles(void);
+
/** Add a point cloud to the collision space */
virtual void addPointCloud(unsigned int n, const double *points, double radius = 0.01);
@@ -78,7 +77,7 @@
virtual void updateRobotModel(unsigned int model_id);
protected:
-
+
class ODECollide2
{
public:
@@ -148,6 +147,8 @@
dGeomID geom;
planning_models::KinematicModel::Link *link;
};
+
+ void freeMemory(void);
std::vector< std::vector< kGeom* > > m_kgeoms;
Modified: pkg/trunk/util/collision_space/src/collision_space/environmentODE.cpp
===================================================================
--- pkg/trunk/util/collision_space/src/collision_space/environmentODE.cpp 2008-07-23 23:53:12 UTC (rev 2015)
+++ pkg/trunk/util/collision_space/src/collision_space/environmentODE.cpp 2008-07-24 00:00:40 UTC (rev 2016)
@@ -34,6 +34,15 @@
#include <collision_space/environmentODE.h>
+void collision_space::EnvironmentModelODE::freeMemory(void)
+{
+ for (unsigned int i = 0 ; i < m_kgeoms.size() ; ++i)
+ for (unsigned int j = 0 ; j < m_kgeoms[i].size() ; ++j)
+ delete m_kgeoms[i][j];
+ if (m_space)
+ dSpaceDestroy(m_space);
+}
+
unsigned int collision_space::EnvironmentModelODE::addRobotModel(planning_models::KinematicModel *model)
{
unsigned int id = collision_space::EnvironmentModel::addRobotModel(model);
@@ -194,3 +203,11 @@
}
m_collide2.setup();
}
+
+void collision_space::EnvironmentModelODE::clearObstacles(void)
+{
+ m_collide2.clear();
+ freeMemory();
+ m_space = dHashSpaceCreate(0);
+ m_collide2.setup();
+}
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <is...@us...> - 2008-07-24 00:38:00
|
Revision: 2023
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=2023&view=rev
Author: isucan
Date: 2008-07-24 00:38:09 +0000 (Thu, 24 Jul 2008)
Log Message:
-----------
setting the extent of the workspace to plan in
Modified Paths:
--------------
pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp
pkg/trunk/robot_models/robot_motion_planning_models/include/planning_models/kinematic.h
pkg/trunk/robot_models/robot_motion_planning_models/src/planning_models/kinematic.cpp
pkg/trunk/services/robot_srvs/srv/KinematicMotionPlan.srv
Modified: pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp 2008-07-24 00:32:12 UTC (rev 2022)
+++ pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp 2008-07-24 00:38:09 UTC (rev 2023)
@@ -173,11 +173,9 @@
tf.setPosition(req.transform.xt, req.transform.yt, req.transform.zt);
tf.setQuaternion(req.transform.xr, req.transform.yr, req.transform.zr, req.transform.w);
-
- // set the 3D space bounding box for planning.
- // if not specified in the request, infer it based on start + goal positions
- // need to know where floating joints are and set these there. also update resolution
-
+ static_cast<SpaceInformationNode*>(p.si)->setPlanningVolume(req.volumeMin.x, req.volumeMin.y, req.volumeMin.z,
+ req.volumeMax.x, req.volumeMax.y, req.volumeMax.z);
+
/* set the starting state */
ompl::SpaceInformationKinematic::StateKinematic_t start = new ompl::SpaceInformationKinematic::StateKinematic(dim);
for (int i = 0 ; i < dim ; ++i)
@@ -209,11 +207,11 @@
}
/* cleanup */
- p.si->clearGoal();
+ p.si->clearGoal();
p.si->clearStartStates(true);
p.mp->clear();
- return true;
+ return true;
}
void addRobotDescriptionFromFile(const char *filename)
@@ -295,6 +293,9 @@
public:
SpaceInformationNode(planning_models::KinematicModel *km) : SpaceInformationKinematic()
{
+ m_km = km;
+ m_divisions = 20.0;
+
m_stateDimension = km->stateDimension;
m_stateComponent.resize(m_stateDimension);
@@ -303,15 +304,35 @@
m_stateComponent[i].type = StateComponent::NORMAL;
m_stateComponent[i].minValue = km->stateBounds[i*2 ];
m_stateComponent[i].maxValue = km->stateBounds[i*2 + 1];
- m_stateComponent[i].resolution = (m_stateComponent[i].maxValue - m_stateComponent[i].minValue) / 20.0;
- if (m_stateComponent[i].resolution == 0.0)
- m_stateComponent[i].resolution = 0.1; // this happens for floating joints
+ m_stateComponent[i].resolution = (m_stateComponent[i].maxValue - m_stateComponent[i].minValue) / m_divisions;
}
}
virtual ~SpaceInformationNode(void)
{
}
+
+ void setPlanningVolume(double x0, double y0, double z0, double x1, double y1, double z1)
+ {
+ for (unsigned int i = 0 ; i < m_km->floatingJoints.size() ; ++i)
+ {
+ int id = m_km->floatingJoints[i];
+ m_stateComponent[id ].minValue = x0;
+ m_stateComponent[id ].maxValue = x1;
+ m_stateComponent[id + 1].minValue = y0;
+ m_stateComponent[id + 1].maxValue = y1;
+ m_stateComponent[id + 2].minValue = z0;
+ m_stateComponent[id + 2].maxValue = z1;
+ for (int j = 0 ; j < 3 ; ++j)
+ m_stateComponent[j + id].resolution = (m_stateComponent[j + id].maxValue - m_stateComponent[j + id].minValue) / m_divisions;
+ }
+ }
+
+ protected:
+
+ double m_divisions;
+ planning_models::KinematicModel *m_km;
+
};
std_msgs::PointCloudFloat32 m_cloud;
Modified: pkg/trunk/robot_models/robot_motion_planning_models/include/planning_models/kinematic.h
===================================================================
--- pkg/trunk/robot_models/robot_motion_planning_models/include/planning_models/kinematic.h 2008-07-24 00:32:12 UTC (rev 2022)
+++ pkg/trunk/robot_models/robot_motion_planning_models/include/planning_models/kinematic.h 2008-07-24 00:38:09 UTC (rev 2023)
@@ -197,6 +197,12 @@
* set to 0, the parameter is unbounded. */
std::vector<double> stateBounds;
+ /** The list of index values where floating joints
+ start. These joints need special attention in motion
+ planning, so the indices are provided here for
+ convenience. */
+ std::vector<int> floatingJoints;
+
/** The model that owns this robot */
KinematicModel *owner;
@@ -235,6 +241,9 @@
/** Cumulative state bounds */
std::vector<double> stateBounds;
+ /** Cumulative list of floating joints */
+ std::vector<int> floatingJoints;
+
protected:
std::vector<Robot*> m_robots;
Modified: pkg/trunk/robot_models/robot_motion_planning_models/src/planning_models/kinematic.cpp
===================================================================
--- pkg/trunk/robot_models/robot_motion_planning_models/src/planning_models/kinematic.cpp 2008-07-24 00:32:12 UTC (rev 2022)
+++ pkg/trunk/robot_models/robot_motion_planning_models/src/planning_models/kinematic.cpp 2008-07-24 00:38:09 UTC (rev 2023)
@@ -122,9 +122,11 @@
for (unsigned int i = 0 ; i < m_robots.size() ; ++i)
{
+ stateBounds.insert(stateBounds.end(), m_robots[i]->stateBounds.begin(), m_robots[i]->stateBounds.end());
+ for (unsigned int j = 0 ; j < m_robots[i]->floatingJoints.size() ; ++j)
+ floatingJoints.push_back(stateDimension + m_robots[i]->floatingJoints[j]);
stateDimension += m_robots[i]->stateDimension;
- stateBounds.insert(stateBounds.end(), m_robots[i]->stateBounds.begin(), m_robots[i]->stateBounds.end());
- }
+ }
}
unsigned int planning_models::KinematicModel::getRobotCount(void) const
@@ -158,6 +160,7 @@
joint->type = Joint::FLOATING;
joint->usedParamEnd = joint->usedParamStart + 3;
robot->stateBounds.insert(robot->stateBounds.end(), 6, 0.0);
+ robot->floatingJoints.push_back(joint->usedParamStart);
break;
case robot_desc::URDF::Link::Joint::FIXED:
joint->type = Joint::FIXED;
Modified: pkg/trunk/services/robot_srvs/srv/KinematicMotionPlan.srv
===================================================================
--- pkg/trunk/services/robot_srvs/srv/KinematicMotionPlan.srv 2008-07-24 00:32:12 UTC (rev 2022)
+++ pkg/trunk/services/robot_srvs/srv/KinematicMotionPlan.srv 2008-07-24 00:38:09 UTC (rev 2023)
@@ -3,6 +3,7 @@
robot_msgs/KinematicState goal_state
float64 allowed_time
std_msgs/TransformQuaternion transform
-std_msgs/Point3DFloat64 volume
+std_msgs/Point3DFloat64 volumeMin
+std_msgs/Point3DFloat64 volumeMax
---
robot_msgs/KinematicPath path
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <sac...@us...> - 2008-07-24 20:26:57
|
Revision: 2071
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=2071&view=rev
Author: sachinchitta
Date: 2008-07-24 20:27:06 +0000 (Thu, 24 Jul 2008)
Log Message:
-----------
Changed MechanismControl class definition to BaseControl class definition
Modified Paths:
--------------
pkg/trunk/mechanism/mechanism_control/include/mechanism_control/base_control.h
pkg/trunk/mechanism/mechanism_control/src/base_control.cpp
pkg/trunk/robot_control_loops/pr2_etherDrive/src/pr2BaseEtherdrive.cpp
Modified: pkg/trunk/mechanism/mechanism_control/include/mechanism_control/base_control.h
===================================================================
--- pkg/trunk/mechanism/mechanism_control/include/mechanism_control/base_control.h 2008-07-24 20:07:32 UTC (rev 2070)
+++ pkg/trunk/mechanism/mechanism_control/include/mechanism_control/base_control.h 2008-07-24 20:27:06 UTC (rev 2071)
@@ -24,8 +24,8 @@
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
//////////////////////////////////////////////////////////////////////////////
-#ifndef MECHANISM_CONTROL_H
-#define MECHANISM_CONTROL_H
+#ifndef BASE_CONTROL_H
+#define BASE_CONTROL_H
#include <mechanism_model/robot.h>
#include <rosthread/mutex.h>
@@ -46,11 +46,11 @@
typedef Controller*(*ControllerAllocationFunc)(const char *);
-class MechanismControl{
+class BaseControl{
public:
- MechanismControl();
+ BaseControl();
void update(); //Must be realtime safe
Modified: pkg/trunk/mechanism/mechanism_control/src/base_control.cpp
===================================================================
--- pkg/trunk/mechanism/mechanism_control/src/base_control.cpp 2008-07-24 20:07:32 UTC (rev 2070)
+++ pkg/trunk/mechanism/mechanism_control/src/base_control.cpp 2008-07-24 20:27:06 UTC (rev 2071)
@@ -37,11 +37,11 @@
using namespace std;
-MechanismControl::MechanismControl(){
+BaseControl::BaseControl(){
this->hw = NULL;
}
-void MechanismControl::init(HardwareInterface *hw){
+void BaseControl::init(HardwareInterface *hw){
this->hw = hw;
@@ -50,7 +50,7 @@
this->initControllers();
}
-void MechanismControl::initRobot(){
+void BaseControl::initRobot(){
r = new Robot("robot");
r->numJoints = NUM_JOINTS;
@@ -72,7 +72,7 @@
}
}
-void MechanismControl::initControllers(){
+void BaseControl::initControllers(){
controller = new BaseController(r,"baseController");
char *c_filename = getenv("ROS_PACKAGE_PATH");
std::stringstream filename;
@@ -82,7 +82,7 @@
}
//This function is called only from the realtime loop. Everything it calls must also be realtime safe.
-void MechanismControl::update(){
+void BaseControl::update(){
//Clear actuator commands
//Process robot model transmissions
Modified: pkg/trunk/robot_control_loops/pr2_etherDrive/src/pr2BaseEtherdrive.cpp
===================================================================
--- pkg/trunk/robot_control_loops/pr2_etherDrive/src/pr2BaseEtherdrive.cpp 2008-07-24 20:07:32 UTC (rev 2070)
+++ pkg/trunk/robot_control_loops/pr2_etherDrive/src/pr2BaseEtherdrive.cpp 2008-07-24 20:27:06 UTC (rev 2071)
@@ -74,7 +74,7 @@
//h.init("BaseEtherdrive.xml"); //Should this be a command-line argument?
//mc.init(h.hardwareInterface, char *namespace or char *init.xml);
- MechanismControl mc;
+ BaseControl mc;
mc.init(h.hw); //If not hard-coded, this is where the ROS namespace or configuration file would be passed in
mc.controller->setVelocity(-0,0,0);
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <tf...@us...> - 2008-07-24 21:55:09
|
Revision: 2081
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=2081&view=rev
Author: tfoote
Date: 2008-07-24 21:55:17 +0000 (Thu, 24 Jul 2008)
Log Message:
-----------
r4358@wgs7: tfoote | 2008-07-24 14:54:15 -0700
test commit
Modified Paths:
--------------
pkg/trunk/clean_rosgazebo.scp
Property Changed:
----------------
/
Property changes on:
___________________________________________________________________
Modified: svk:merge
- 7c5ec9ba-cbf4-4c9d-918a-458b6f267dae:/wgpersonalrobots:4250
+ 7c5ec9ba-cbf4-4c9d-918a-458b6f267dae:/wgpersonalrobots:4358
Modified: pkg/trunk/clean_rosgazebo.scp
===================================================================
--- pkg/trunk/clean_rosgazebo.scp 2008-07-24 21:52:57 UTC (rev 2080)
+++ pkg/trunk/clean_rosgazebo.scp 2008-07-24 21:55:17 UTC (rev 2081)
@@ -1,5 +1,5 @@
#!/bin/bash
-
+
#clean controllers
(cd controllers/genericControllers ; rm -f lib/* ;make clean)
(cd controllers/pr2Controllers ; rm -f lib/* ;make clean)
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <is...@us...> - 2008-07-24 23:14:42
|
Revision: 2093
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=2093&view=rev
Author: isucan
Date: 2008-07-24 23:14:51 +0000 (Thu, 24 Jul 2008)
Log Message:
-----------
changed motion planning request format and began work on new test node
Modified Paths:
--------------
pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp
pkg/trunk/services/robot_srvs/srv/KinematicMotionPlan.srv
pkg/trunk/util/collision_space/include/collision_space/environment.h
Added Paths:
-----------
pkg/trunk/motion_planning/plan_kinematic_path/
pkg/trunk/motion_planning/plan_kinematic_path/CMakeLists.txt
pkg/trunk/motion_planning/plan_kinematic_path/Makefile
pkg/trunk/motion_planning/plan_kinematic_path/manifest.xml
pkg/trunk/motion_planning/plan_kinematic_path/src/
pkg/trunk/motion_planning/plan_kinematic_path/src/plan_kinematic_path.cpp
Modified: pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp 2008-07-24 23:08:38 UTC (rev 2092)
+++ pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp 2008-07-24 23:14:51 UTC (rev 2093)
@@ -83,7 +83,6 @@
#include <ros/node.h>
#include <std_msgs/PointCloudFloat32.h>
-#include <robot_msgs/KinematicPath.h>
#include <robot_srvs/KinematicMotionPlan.h>
#include <urdf/URDF.h>
Added: pkg/trunk/motion_planning/plan_kinematic_path/CMakeLists.txt
===================================================================
--- pkg/trunk/motion_planning/plan_kinematic_path/CMakeLists.txt (rev 0)
+++ pkg/trunk/motion_planning/plan_kinematic_path/CMakeLists.txt 2008-07-24 23:14:51 UTC (rev 2093)
@@ -0,0 +1,4 @@
+cmake_minimum_required(VERSION 2.6)
+include(rosbuild)
+rospack(plan_kinematic_path)
+rospack_add_executable(plan_kinematic_path src/plan_kinematic_path.cpp)
Added: pkg/trunk/motion_planning/plan_kinematic_path/Makefile
===================================================================
--- pkg/trunk/motion_planning/plan_kinematic_path/Makefile (rev 0)
+++ pkg/trunk/motion_planning/plan_kinematic_path/Makefile 2008-07-24 23:14:51 UTC (rev 2093)
@@ -0,0 +1 @@
+include $(shell rospack find mk)/cmake.mk
Added: pkg/trunk/motion_planning/plan_kinematic_path/manifest.xml
===================================================================
--- pkg/trunk/motion_planning/plan_kinematic_path/manifest.xml (rev 0)
+++ pkg/trunk/motion_planning/plan_kinematic_path/manifest.xml 2008-07-24 23:14:51 UTC (rev 2093)
@@ -0,0 +1,9 @@
+<package>
+ <description>Test package that can plan a kinematic path</description>
+ <author>Ioan A. Sucan</author>
+ <license>BSD</license>
+ <depend package="roscpp" />
+ <depend package="robot_msgs" />
+ <depend package="robot_srvs" />
+ <depend package="xmlparam" />
+</package>
Added: pkg/trunk/motion_planning/plan_kinematic_path/src/plan_kinematic_path.cpp
===================================================================
--- pkg/trunk/motion_planning/plan_kinematic_path/src/plan_kinematic_path.cpp (rev 0)
+++ pkg/trunk/motion_planning/plan_kinematic_path/src/plan_kinematic_path.cpp 2008-07-24 23:14:51 UTC (rev 2093)
@@ -0,0 +1,73 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2008, Willow Garage, Inc.
+* All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* * Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* * Redistributions in binary form must reproduce the above
+* copyright notice, this list of conditions and the following
+* disclaimer in the documentation and/or other materials provided
+* with the distribution.
+* * Neither the name of the Willow Garage nor the names of its
+* contributors may be used to endorse or promote products derived
+* from this software without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+#include <ros/node.h>
+#include <robot_srvs/KinematicMotionPlan.h>
+
+class PlanKinematicPath : public ros::node
+{
+public:
+
+ PlanKinematicPath(void) : ros::node("plan_kinematic_path")
+ {
+ }
+
+ void runTest1(void)
+ {
+ robot_srvs::KinematicMotionPlan::request req;
+ robot_srvs::KinematicMotionPlan::response res;
+
+ if (ros::service::call("plan_kinematic_path", req, res))
+ {
+
+
+ }
+ else
+ fprintf(stderr, "Service 'plan_kinematic_path' failed\n");
+ }
+
+
+};
+
+
+int main(int argc, char **argv)
+{
+ ros::init(argc, argv);
+
+ PlanKinematicPath plan;
+ plan.runTest1();
+ plan.shutdown();
+
+ return 0;
+}
Modified: pkg/trunk/services/robot_srvs/srv/KinematicMotionPlan.srv
===================================================================
--- pkg/trunk/services/robot_srvs/srv/KinematicMotionPlan.srv 2008-07-24 23:08:38 UTC (rev 2092)
+++ pkg/trunk/services/robot_srvs/srv/KinematicMotionPlan.srv 2008-07-24 23:14:51 UTC (rev 2093)
@@ -2,7 +2,6 @@
robot_msgs/KinematicState start_state
robot_msgs/KinematicState goal_state
float64 allowed_time
-std_msgs/TransformQuaternion transform
std_msgs/Point3DFloat64 volumeMin
std_msgs/Point3DFloat64 volumeMax
---
Modified: pkg/trunk/util/collision_space/include/collision_space/environment.h
===================================================================
--- pkg/trunk/util/collision_space/include/collision_space/environment.h 2008-07-24 23:08:38 UTC (rev 2092)
+++ pkg/trunk/util/collision_space/include/collision_space/environment.h 2008-07-24 23:14:51 UTC (rev 2093)
@@ -52,6 +52,7 @@
EnvironmentModel(void)
{
+ m_selfCollision = true;
}
virtual ~EnvironmentModel(void)
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <hsu...@us...> - 2008-07-25 01:25:41
|
Revision: 2111
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=2111&view=rev
Author: hsujohnhsu
Date: 2008-07-25 01:25:49 +0000 (Fri, 25 Jul 2008)
Log Message:
-----------
updates to mechanism control object, with some patches to armcontroller to make it compile.
Modified Paths:
--------------
pkg/trunk/controllers/pr2Controllers/include/pr2Controllers/ArmController.h
pkg/trunk/controllers/pr2Controllers/src/ArmController.cpp
pkg/trunk/drivers/simulator/gazebo_hardware/include/gazebo_hardware/gazebo_hardware.h
pkg/trunk/drivers/simulator/gazebo_hardware/src/gazebo_hardware.cpp
pkg/trunk/mechanism/mechanism_control/src/gazebo_control.cpp
Modified: pkg/trunk/controllers/pr2Controllers/include/pr2Controllers/ArmController.h
===================================================================
--- pkg/trunk/controllers/pr2Controllers/include/pr2Controllers/ArmController.h 2008-07-25 01:24:12 UTC (rev 2110)
+++ pkg/trunk/controllers/pr2Controllers/include/pr2Controllers/ArmController.h 2008-07-25 01:25:49 UTC (rev 2111)
@@ -16,11 +16,23 @@
#include <iostream>
+// to be replaced by xml
#include <pr2Core/pr2Core.h>
+
+// generic controllers
#include <genericControllers/Controller.h>
#include <genericControllers/JointController.h>
+
+// robot model
#include <mechanism_model/joint.h>
+#include <mechanism_model/robot.h>
+
+// kinematics library
#include <libKDL/kdl_kinematics.h>
+
+// for temporary gettimeofday call, will be passed in from hardware interface and deprecate
+#include <sys/time.h>
+
namespace controller
{
class ArmController : Controller
@@ -46,13 +58,21 @@
* \param
*/
ArmController();
-
+ ArmController(Robot *robot, std::string name);
+ ArmController(Robot *robot);
+ ArmController(Robot *robot, std::string name,int armNumJoints, int jcToRobotJointMap[], int jcToHIActuatorMap[]);
+
/*!
* \brief Destructor.
*/
~ArmController( );
/*!
+ * \brief load XML file
+ */
+ controllerErrorCode loadXML(std::string filename);
+
+ /*!
* \brief initialize controller variables
*/
void init();
@@ -64,6 +84,13 @@
*/
void initJoint(int jointNum, double pGain, double iGain, double dGain, double iMax, double iMin, controllerControlMode mode, double time,double maxEffort,double minEffort, mechanism::Joint *joint);
+ //TEMPORARY
+ /*!
+ * \brief get system time, want to get simulator time for simulation.
+ *
+ */
+ double getTime();
+
//TEMPORARY
/*!
* \brief Call after initializing individual joints to initialize arm as a whole
@@ -352,7 +379,7 @@
/*!
* \brief Update controller
*/
- void update( );
+ void update();
//---------------------------------------------------------------------------------//
// MISC CALLS
@@ -366,15 +393,60 @@
bool enabled; /**<Is the arm controller active?>*/
controllerControlMode controlMode; /**< Arm controller control mode >*/
- std::string name; /**<Namespace identifier for ROS>*/
+ /*!
+ * \brief initialize joint controllers
+ */
+ void initJointControllers();
+ JointController *armJointControllers; /**< Lower level control done by JointControllers>*/
- JointController lowerControl[ARM_MAX_JOINTS]; /**< Lower level control done by JointControllers>*/
-
double cmdPos[6]; /**<Last commanded cartesian position>*/
double cmdVel[6]; /**<Last commanded cartesian velocity>*/
PR2_kinematics pr2_kin; /**<PR2 kinematics>*/
+ double pGain; /**< Proportional gain for position control */
+
+ double iGain; /**< Integral gain for position control */
+
+ double dGain; /**< Derivative gain for position control */
+
+ double iMax; /**< Max integral error term */
+
+ double iMin; /**< Min integral error term */
+
+ double maxEffort; /**< maximum effort */
+
+ double minEffort; /**< maximum effort */
+
+ double xDotCmd; /**< Forward speed cmd */
+
+ double yDotCmd; /**< Sideways speed cmd (motion to the left is positive) */
+
+ double yawDotCmd; /**< Rotational speed cmd (motion counter-clockwise is positive) */
+
+ double xDotNew; /**< New forward speed cmd */
+
+ double yDotNew; /**< New sideways speed cmd (motion to the left is positive) */
+
+ double yawDotNew; /**< New rotational speed cmd (motion counter-clockwise is positive) */
+
+ double maxXDot;
+
+ double maxYDot;
+
+ double maxYawDot;
+
+ Robot *robot;
+
+ std::map<std::string,std::string> paramMap;
+
+ std::string name; /**<Namespace identifier for ROS>*/
+
+ void loadParam(std::string label, double &value);
+
+ void loadParam(std::string label, int &value);
+
+
};
}
Modified: pkg/trunk/controllers/pr2Controllers/src/ArmController.cpp
===================================================================
--- pkg/trunk/controllers/pr2Controllers/src/ArmController.cpp 2008-07-25 01:24:12 UTC (rev 2110)
+++ pkg/trunk/controllers/pr2Controllers/src/ArmController.cpp 2008-07-25 01:25:49 UTC (rev 2111)
@@ -1,4 +1,5 @@
#include <pr2Controllers/ArmController.h>
+#define NDOF 6 // accounts for x,y,z,roll,pitch,yaw
using namespace controller;
/***************************************************/
/*! \class controller::ArmController
@@ -40,31 +41,157 @@
ArmController::ArmController()
{
- //Default construction of JointControllers should init them to blank controllers
+ this->robot = NULL;
+ this->name = "armController";
}
-
-ArmController::~ArmController( )
+ArmController::ArmController(Robot *robot)
{
+ this->robot = robot;
+ this->name = "armController";
}
+ArmController::ArmController(Robot *robot, std::string name)
+{
+ this->robot = robot;
+ this->name = name;
+}
+
+ArmController::ArmController(Robot *robot, std::string name,int armNumJoints, int jcToRobotJointMap[], int jcToHIActuatorMap[])
+{
+ this->robot = robot;
+ this->name = "armController";
+}
+
+ArmController::~ArmController()
+{
+}
+
+
+controllerErrorCode ArmController::loadXML(std::string filename)
+{
+ robot_desc::URDF model;
+ int exists = 0;
+
+ if(!model.loadFile(filename.c_str()))
+ return CONTROLLER_MODE_ERROR;
+
+ const robot_desc::URDF::Data &data = model.getData();
+
+ std::vector<std::string> types;
+ std::vector<std::string> names;
+ std::vector<std::string>::iterator iter;
+
+ data.getDataTagTypes(types);
+
+ for(iter = types.begin(); iter != types.end(); iter++){
+ if(*iter == "controller"){
+ exists = 1;
+ break;
+ }
+ }
+
+ if(!exists)
+ return CONTROLLER_MODE_ERROR;
+
+ exists = 0;
+
+ for(iter = names.begin(); iter != names.end(); iter++){
+ if(*iter == this->name){
+ exists = 1;
+ break;
+ }
+ }
+ paramMap = data.getDataTagValues("controller",this->name);
+
+ loadParam("pGain",pGain);
+ printf("BC:: %f\n",pGain);
+ loadParam("dGain",dGain);
+ loadParam("iGain",iGain);
+
+ loadParam("maxXDot",maxXDot);
+ loadParam("maxYDot",maxYDot);
+ loadParam("maxYawDot",maxYawDot);
+
+ return CONTROLLER_ALL_OK;
+}
+
+
void ArmController::init()
{
// to be filled in here
- // initJointControllers();
+ initJointControllers();
}
+void ArmController::initJointControllers()
+{
+ this->armJointControllers = new JointController[ARM_MAX_JOINTS];
+ /**************************************************************************************/
+ /* */
+ /* MAPPING BETWEEN: armController->jointController array and robot->joint array */
+ /* */
+ /* note: this is how the arm's joint controller arrays knows when robot->joint */
+ /* to use */
+ /* */
+ /**************************************************************************************/
+
+ for (int ii=0; ii < ARM_MAX_JOINTS; ii++) {
+ // FIXME: hardcoded values for gazebo for now
+ //armJointControllers[ii].init(pGain, iGain, dGain, iMax, iMin, ETHERDRIVE_SPEED, getTime(), maxEffort, minEffort, &(robot->joint[ii]));
+ armJointControllers[ii].init( 1000, 0, 0, 500, -500, controller::CONTROLLER_POSITION, getTime(), 1000, -1000, &(robot->joint[ii]));
+ armJointControllers[ii].enableController();
+ }
+ /***************************************************************************************/
+ /* */
+ /* initialize controllers */
+ /* hardcoded values, should be replaced by reading controllers.xml file */
+ /* TODO: assigned by xml fils and param servers */
+ /* */
+ /***************************************************************************************/
+#if 0
+ // initialize each jointController jc[i], associate with a joint joint[i]
+ for (int i = 0;i<PR2::MAX_JOINTS;i++){
+ jc[i] = new controller::JointController();
+ jc[i]->init(joint[i]);
+ }
+
+ //Explicitly initialize the controllers we wish to use. Don't forget to set the controllers to torque mode in the world file!
+ armJointController[PR2::ARM_L_PAN]->initJoint(100,0,0,500,-500, controller::CONTROLLER_POSITION,time,-100,100,joint[PR2::ARM_L_PAN]);
+ armJointController[PR2::ARM_L_SHOULDER_PITCH]->init(1000,0,0,500,-500, controller::CONTROLLER_POSITION,time,-1000,1000,joint[PR2::ARM_L_SHOULDER_PITCH]);
+ armJointController[PR2::ARM_L_SHOULDER_ROLL]->init(100,0,0,500,-500, controller::CONTROLLER_POSITION,time,-100,100,joint[PR2::ARM_L_SHOULDER_ROLL]);
+ armJointController[PR2::ARM_L_ELBOW_PITCH]->init(300,0,0,500,-500, controller::CONTROLLER_POSITION,time,-100,100,joint[PR2::ARM_L_ELBOW_PITCH]);
+ armJointController[PR2::ARM_L_ELBOW_ROLL]->init(100,0,0,500,-500, controller::CONTROLLER_POSITION,time,-100,100,joint[PR2::ARM_L_ELBOW_ROLL]);
+ armJointController[PR2::ARM_L_WRIST_PITCH]->init(100,0,0,500,-500, controller::CONTROLLER_POSITION,time,-100,100,joint[PR2::ARM_L_WRIST_PITCH]);
+ armJointController[PR2::ARM_L_WRIST_ROLL]->init(100,0,0,500,-500, controller::CONTROLLER_POSITION,time,-100,100,joint[PR2::ARM_L_WRIST_ROLL]);
+
+ armJointController[PR2::ARM_R_PAN]->init(100,0,0,500,-500, controller::CONTROLLER_POSITION,time,-100,100,joint[PR2::ARM_R_PAN]);
+ armJointController[PR2::ARM_R_SHOULDER_PITCH]->init(1000,0,0,500,-500, controller::CONTROLLER_POSITION,time,-1000,1000,joint[PR2::ARM_R_SHOULDER_PITCH]);
+ armJointController[PR2::ARM_R_SHOULDER_ROLL]->init(100,0,0,500,-500, controller::CONTROLLER_POSITION,time,-100,100,joint[PR2::ARM_R_SHOULDER_ROLL]);
+ armJointController[PR2::ARM_R_ELBOW_PITCH]->init(300,0,0,500,-500, controller::CONTROLLER_POSITION,time,-100,100,joint[PR2::ARM_R_ELBOW_PITCH]);
+ armJointController[PR2::ARM_R_ELBOW_ROLL]->init(100,0,0,500,-500, controller::CONTROLLER_POSITION,time,-100,100,joint[PR2::ARM_R_ELBOW_ROLL]);
+ armJointController[PR2::ARM_R_WRIST_PITCH]->init(100,0,0,500,-500, controller::CONTROLLER_POSITION,time,-100,100,joint[PR2::ARM_R_WRIST_PITCH]);
+ armJointController[PR2::ARM_R_WRIST_ROLL]->init(100,0,0,500,-500, controller::CONTROLLER_POSITION,time,-100,100,joint[PR2::ARM_R_WRIST_ROLL]);
+#endif
+}
+
+double ArmController::getTime()
+{
+ struct timeval t;
+ gettimeofday( &t, 0);
+ return (double) (t.tv_usec *1e-6 + t.tv_sec);
+}
+
void ArmController::initJoint(int jointNum, double PGain, double IGain, double DGain, double IMax, double IMin, controllerControlMode mode, double time, double maxEffort,double minEffort, mechanism::Joint *joint)
{
- lowerControl[jointNum].init(PGain, IGain, DGain, IMax, IMin, CONTROLLER_DISABLED, time, maxEffort, minEffort, joint); //Initialize joint, but keep in disabled state
+ armJointControllers[jointNum].init(PGain, IGain, DGain, IMax, IMin, CONTROLLER_DISABLED, time, maxEffort, minEffort, joint); //Initialize joint, but keep in disabled state
}
controllerErrorCode ArmController::initArm(controllerControlMode mode)
{
//Reset commanded positions
- for(int i =0 ;i<6;i++){
+ for(int i =0 ;i< NDOF ;i++){
cmdPos[i] = 0.0;
cmdVel[i] = 0.0;
}
@@ -72,7 +199,7 @@
//Set mode
controlMode = mode;
for(int i =0;i<ARM_MAX_JOINTS;i++){
- lowerControl[i].setMode(mode); //Set underlying jointControllers to position control
+ armJointControllers[i].setMode(mode); //Set underlying jointControllers to position control
}
//Turn on the controller
@@ -87,7 +214,7 @@
controllerControlMode ArmController::setMode(controllerControlMode mode)
{
for(int i = 0;i<ARM_MAX_JOINTS;i++){
- lowerControl[i].setMode(mode);
+ armJointControllers[i].setMode(mode);
}
return CONTROLLER_MODE_SET;
}
@@ -96,7 +223,7 @@
{
enabled = true;
for(int i = 0;i<ARM_MAX_JOINTS;i++){
- lowerControl[i].enableController();
+ armJointControllers[i].enableController();
}
return CONTROLLER_ENABLED;
}
@@ -105,7 +232,7 @@
{
enabled = false;
for(int i = 0;i<ARM_MAX_JOINTS;i++){
- lowerControl[i].disableController();
+ armJointControllers[i].disableController();
}
return CONTROLLER_DISABLED;
@@ -120,7 +247,7 @@
void ArmController::checkForSaturation(bool* status[])
{
for(int i= 0;i<ARM_MAX_JOINTS;i++){
- *status[i] = lowerControl[i].checkForSaturation();
+ *status[i] = armJointControllers[i].checkForSaturation();
}
}
@@ -159,16 +286,16 @@
}
//Perform inverse kinematics
- if (pr2_kin.IK(q_init, f, q_out)){
+ if (this->pr2_kin.IK(q_init, f, q_out)){
// cout<<"IK result:"<<q_out<<endl;
- }else{
+ }else{
//cout<<"Could not compute Inv Kin."<<endl;
return CONTROLLER_JOINT_ERROR;
}
//------ checking that IK returned a valid soln -----
KDL::Frame f_ik;
- if (pr2_kin.FK(q_out,f_ik))
+ if (this->pr2_kin.FK(q_out,f_ik))
{
// cout<<"End effector after IK:"<<f_ik<<endl;
}
@@ -179,7 +306,7 @@
//Record commands and shove to arm
for(int ii = 0; ii < ARM_MAX_JOINTS; ii++){
- lowerControl[ii].setPosCmd(q_out(ii));
+ armJointControllers[ii].setPosCmd(q_out(ii));
//std::cout<<"*"<<q_out(ii);
}
//std::cout<<std::endl;
@@ -187,12 +314,12 @@
}
controllerErrorCode ArmController::getHandCartesianPosCmd(double *x, double *y, double *z, double *roll, double *pitch, double *yaw)
{
- *x = cmdPos[0];
- *y = cmdPos[1];
- *z = cmdPos[2];
- *roll = cmdPos[3];
+ *x = cmdPos[0];
+ *y = cmdPos[1];
+ *z = cmdPos[2];
+ *roll = cmdPos[3];
*pitch = cmdPos[4];
- *yaw = cmdPos[5];
+ *yaw = cmdPos[5];
return CONTROLLER_ALL_OK;
}
controllerErrorCode ArmController::getHandCartesianPosAct(double *x, double *y, double *z, double *roll, double *pitch, double *yaw)
@@ -201,13 +328,13 @@
KDL::JntArray q_jts(ARM_MAX_JOINTS);
//Read current joint locations
for(int i = 0;i<ARM_MAX_JOINTS;i++){
- lowerControl[i].getPosAct(&pos);
+ armJointControllers[i].getPosAct(&pos);
q_jts(i) = pos;
}
//Perform forward kinematics to get cartesian location
KDL::Frame fk;
- if (pr2_kin.FK(q_jts,fk))
+ if (this->pr2_kin.FK(q_jts,fk))
{
// cout<<"End effector:"<<fk<<endl;
}
@@ -234,9 +361,9 @@
controllerErrorCode ArmController::getHandOrientationCmd(double *roll, double *pitch, double *yaw)
{
- *roll = cmdPos[3];
+ *roll = cmdPos[3];
*pitch = cmdPos[4];
- *yaw = cmdPos[5];
+ *yaw = cmdPos[5];
return CONTROLLER_ALL_OK;
}
@@ -280,7 +407,7 @@
controllerErrorCode current;
if(controlMode!= CONTROLLER_POSITION) return CONTROLLER_MODE_ERROR; //TODO implement errors?
for (int i = 0;i<getNumJoints();i++){
- current = lowerControl[i].setPosCmd(angles[i]);
+ current = armJointControllers[i].setPosCmd(angles[i]);
// std::cout<<i<<":"<<current<<std::endl;
if(current!=CONTROLLER_ALL_OK) error = current;
}
@@ -292,7 +419,7 @@
controllerErrorCode current;
for (int i = 0;i<getNumJoints();i++){
- current = lowerControl[i].getPosCmd(angles[i]); //If we find even a single error,return error for overall
+ current = armJointControllers[i].getPosCmd(angles[i]); //If we find even a single error,return error for overall
if(current!=CONTROLLER_ALL_OK) error = current;
}
return error;
@@ -304,7 +431,7 @@
controllerErrorCode current;
for (int i = 0;i<getNumJoints();i++){
- current = lowerControl[i].getPosAct(angles[i]); //If we find even a single error,return error for overall
+ current = armJointControllers[i].getPosAct(angles[i]); //If we find even a single error,return error for overall
if(current!=CONTROLLER_ALL_OK) error = current;
}
@@ -314,19 +441,19 @@
controllerErrorCode ArmController::setOneArmJointPos(int numJoint, double angle)
{
if(numJoint<0 || numJoint > ARM_MAX_JOINTS) return CONTROLLER_JOINT_ERROR; //Index out of bounds
- return lowerControl[numJoint].setPosCmd(angle);
+ return armJointControllers[numJoint].setPosCmd(angle);
}
controllerErrorCode ArmController::getOneArmJointPosCmd(int numJoint, double *angle)
{
if(numJoint<0 || numJoint > ARM_MAX_JOINTS) return CONTROLLER_JOINT_ERROR; //Index out of bounds
- return lowerControl[numJoint].getPosCmd(angle);
+ return armJointControllers[numJoint].getPosCmd(angle);
}
controllerErrorCode ArmController::getOneArmJointPosAct(int numJoint, double *angle)
{
if(numJoint<0 || numJoint > ARM_MAX_JOINTS) return CONTROLLER_JOINT_ERROR; //Index out of bounds
- return lowerControl[numJoint].getPosAct(angle);
+ return armJointControllers[numJoint].getPosAct(angle);
}
//---------------------------------------------------------------------------------//
@@ -339,7 +466,7 @@
controllerErrorCode current;
if(controlMode!= CONTROLLER_TORQUE) return CONTROLLER_MODE_ERROR; //TODO implement errors?
for (int i = 0;i<getNumJoints();i++){
- current = lowerControl[i].setTorqueCmd(torque[i]); //If we find even a single error, complete the set position and return error for overall
+ current = armJointControllers[i].setTorqueCmd(torque[i]); //If we find even a single error, complete the set position and return error for overall
if(current!=CONTROLLER_ALL_OK) error = current;
}
@@ -352,7 +479,7 @@
controllerErrorCode current;
for (int i = 0;i<getNumJoints();i++){
- current = lowerControl[i].getTorqueCmd(torque[i]); //If we find even a single error,return error for overall
+ current = armJointControllers[i].getTorqueCmd(torque[i]); //If we find even a single error,return error for overall
if(current!=CONTROLLER_ALL_OK) error = current;
}
@@ -365,7 +492,7 @@
controllerErrorCode current;
for (int i = 0;i<getNumJoints();i++){
- current = lowerControl[i].getTorqueAct(torque[i]); //If we find even a single error,return error for overall
+ current = armJointControllers[i].getTorqueAct(torque[i]); //If we find even a single error,return error for overall
if(current!=CONTROLLER_ALL_OK) error = current;
}
return error;
@@ -375,21 +502,21 @@
controllerErrorCode ArmController::setOneArmJointTorque(int numJoint,double torque)
{
if(numJoint<0 || numJoint > ARM_MAX_JOINTS) return CONTROLLER_JOINT_ERROR; //Index out of bounds
- return lowerControl[numJoint].setTorqueCmd(torque);
+ return armJointControllers[numJoint].setTorqueCmd(torque);
}
controllerErrorCode ArmController::getArmJointTorqueCmd(int numJoint, double *torque)
{
if(numJoint<0 || numJoint > ARM_MAX_JOINTS) return CONTROLLER_JOINT_ERROR; //Index out of bounds
- return lowerControl[numJoint].getTorqueCmd(torque);
+ return armJointControllers[numJoint].getTorqueCmd(torque);
}
controllerErrorCode ArmController::getArmJointTorqueAct(int numJoint, double *torque)
{
if(numJoint<0 || numJoint > ARM_MAX_JOINTS) return CONTROLLER_JOINT_ERROR; //Index out of bounds
- return lowerControl[numJoint].getTorqueAct(torque);
+ return armJointControllers[numJoint].getTorqueAct(torque);
}
@@ -404,7 +531,7 @@
controllerErrorCode current;
if(controlMode!= CONTROLLER_VELOCITY) return CONTROLLER_MODE_ERROR; //TODO implement errors?
for (int i = 0;i<getNumJoints();i++){
- current = lowerControl[i].setVelCmd(speed[i]); //If we find even a single error, complete the set position and return error for overall
+ current = armJointControllers[i].setVelCmd(speed[i]); //If we find even a single error, complete the set position and return error for overall
if(current!=CONTROLLER_ALL_OK) error = current;
}
return error;
@@ -416,7 +543,7 @@
controllerErrorCode current;
for (int i = 0;i<getNumJoints();i++){
- current = lowerControl[i].getVelCmd(speed[i]); //If we find even a single error,return error for overall
+ current = armJointControllers[i].getVelCmd(speed[i]); //If we find even a single error,return error for overall
if(current!=CONTROLLER_ALL_OK) error = current;
}
return error;
@@ -428,7 +555,7 @@
controllerErrorCode current;
for (int i = 0;i<getNumJoints();i++){
- current = lowerControl[i].getVelAct(speed[i]); //If we find even a single error,return error for overall
+ current = armJointControllers[i].getVelAct(speed[i]); //If we find even a single error,return error for overall
if(current!=CONTROLLER_ALL_OK) error = current;
}
return error;
@@ -437,18 +564,18 @@
controllerErrorCode ArmController::setOneArmJointSpeed(int numJoint, double speed){
if(numJoint<0 || numJoint > ARM_MAX_JOINTS) return CONTROLLER_JOINT_ERROR; //Index out of bounds
- return lowerControl[numJoint].setVelCmd(speed);
+ return armJointControllers[numJoint].setVelCmd(speed);
}
controllerErrorCode ArmController::getOneArmJointSpeedCmd(int numJoint, double *speed)
{
if(numJoint<0 || numJoint > ARM_MAX_JOINTS) return CONTROLLER_JOINT_ERROR; //Index out of bounds
- return lowerControl[numJoint].getVelCmd(speed);
+ return armJointControllers[numJoint].getVelCmd(speed);
}
controllerErrorCode ArmController::getOneArmJointSpeedAct(int numJoint, double *speed)
{
if(numJoint<0 || numJoint > ARM_MAX_JOINTS) return CONTROLLER_JOINT_ERROR; //Index out of bounds
- return lowerControl[numJoint].getVelAct(speed);
+ return armJointControllers[numJoint].getVelAct(speed);
}
@@ -481,21 +608,30 @@
{
if(!enabled){
for(int i = 0;i<ARM_MAX_JOINTS;i++){
- lowerControl[i].disableController();
+ armJointControllers[i].disableController();
}
} //Make sure controller is enabled. Otherwise, tell all jointcontrollers to shut down
for(int i = 0;i<ARM_MAX_JOINTS;i++){
- lowerControl[i].update();
+ armJointControllers[i].update();
}
+}
+void ArmController::loadParam(std::string label, double ¶m)
+{
+ if(paramMap.find(label) != paramMap.end()) // if parameter value has been initialized in the xml file, set internal parameter value
+ param = atof(paramMap[label].c_str());
}
+void ArmController::loadParam(std::string label, int ¶m)
+{
+ if(paramMap.find(label) != paramMap.end())
+ param = atoi(paramMap[label].c_str());
+}
-//---------------------------------------------------------------------------------//
-// MISC CALLS
-//---------------------------------------------------------------------------------//
-int ArmController::getNumJoints(){
- return ARM_MAX_JOINTS;
+int ArmController::getNumJoints()
+{
+ return ARM_MAX_JOINTS;
}
+
Modified: pkg/trunk/drivers/simulator/gazebo_hardware/include/gazebo_hardware/gazebo_hardware.h
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_hardware/include/gazebo_hardware/gazebo_hardware.h 2008-07-25 01:24:12 UTC (rev 2110)
+++ pkg/trunk/drivers/simulator/gazebo_hardware/include/gazebo_hardware/gazebo_hardware.h 2008-07-25 01:25:49 UTC (rev 2111)
@@ -79,11 +79,6 @@
void updateState();
/*!
- * \brief tick send most recent motor commands and retrieve updates. This command must be run at a sufficient rate or else the motors will be disabled.
- */
- void tick();
-
- /*!
* \brief Read the command values from the hardware interface and send them out to the actual motors
\param HardwareInterface* hw pointer to the hardware interface
*/
Modified: pkg/trunk/drivers/simulator/gazebo_hardware/src/gazebo_hardware.cpp
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_hardware/src/gazebo_hardware.cpp 2008-07-25 01:24:12 UTC (rev 2110)
+++ pkg/trunk/drivers/simulator/gazebo_hardware/src/gazebo_hardware.cpp 2008-07-25 01:25:49 UTC (rev 2111)
@@ -175,6 +175,10 @@
hardwareInterface->actuator[jointId[ii]].state.encoderCount = GAZEBO_POS_TO_ENCODER*( pr2PTZCameraRightIface->data->tilt );
pr2PTZCameraRightIface->Unlock();
}
+ else
+ {
+ //FIXME: default action??
+ }
//fprintf(stderr,"edh:: %d\n",hardwareInterface->actuator[jointId[ii]].state.encoderCount);
}
@@ -335,40 +339,6 @@
}
}
-void GazeboHardware::tick() {
- for(int ii = 0; ii < numActuators; ii++)
- {
- if (boardLookUp[ii] == 0)
- {
- //pr2ActarrayIface->Lock(1);
- //pr2ActarrayIface->data->actuators[portLookUp[ii]].cmdEnableMotor = motorsOn;
- //pr2ActarrayIface->Unlock();
- }
- else if (boardLookUp[ii] == 1)
- {
- //pr2GripperLeftIface->Lock(1);
- //pr2GripperLeftIface->data->cmdEnableMotor = motorsOn;
- //pr2GripperLeftIface->Unlock();
- }
- else if (boardLookUp[ii] == 2)
- {
- //pr2GripperRightIface->Lock(1);
- //pr2GripperRightIface->data->cmdEnableMotor = motorsOn;
- //pr2GripperRightIface->Unlock();
- }
- else if (boardLookUp[ii] == 3)
- {
- }
- else if (boardLookUp[ii] == 4)
- {
- }
- else
- {
- //FIXME: default action??
- }
- }
-}
-
GazeboHardware::~GazeboHardware()
{
// printf("Switching off motors \n");
Modified: pkg/trunk/mechanism/mechanism_control/src/gazebo_control.cpp
===================================================================
--- pkg/trunk/mechanism/mechanism_control/src/gazebo_control.cpp 2008-07-25 01:24:12 UTC (rev 2110)
+++ pkg/trunk/mechanism/mechanism_control/src/gazebo_control.cpp 2008-07-25 01:25:49 UTC (rev 2111)
@@ -65,6 +65,7 @@
r->joint = new Joint[PR2::MAX_JOINTS];
r->link = new Link[PR2::MAX_JOINTS];
+ // MAPPING BETWEEN hwInterface->actuators and robot->joints
// assign transmissions for all joints
for(int ii=0; ii<PR2::MAX_JOINTS; ii++){
r->transmission[ii].actuator = &(hardwareInterface->actuator[ii]);
@@ -81,13 +82,59 @@
}
void GazeboMechanismControl::initControllers(){
- baseController = new BaseController(r,"baseController");
- leftArmController = new ArmController(); //r,"leftArmController");
- rightArmController = new ArmController(); //r,"rightArmController");
- //headController = new HeadController(r,"headController");
- //laserScannerController = new LaserScannerController(r,"headController");
- //spineController = new SpineController(r,"headController");
+ // assuming that: robot joint array == hardware interface actuator array
+ // hard coded mapping between jointcontroller array and robot joint array
+ int baseMapToRobotJointIndex[] = {
+ PR2::CASTER_FL_STEER , PR2::CASTER_FL_DRIVE_L , PR2::CASTER_FL_DRIVE_R ,
+ PR2::CASTER_FR_STEER , PR2::CASTER_FR_DRIVE_L , PR2::CASTER_FR_DRIVE_R ,
+ PR2::CASTER_RL_STEER , PR2::CASTER_RL_DRIVE_L , PR2::CASTER_RL_DRIVE_R ,
+ PR2::CASTER_RR_STEER , PR2::CASTER_RR_DRIVE_L , PR2::CASTER_RR_DRIVE_R };
+ int leftArmMapToRobotJointIndex[] = {
+ PR2::ARM_L_PAN , PR2::ARM_L_SHOULDER_PITCH, PR2::ARM_L_SHOULDER_ROLL ,
+ PR2::ARM_L_ELBOW_PITCH , PR2::ARM_L_ELBOW_ROLL , PR2::ARM_L_WRIST_PITCH ,
+ PR2::ARM_L_WRIST_ROLL };
+ int rightArmMapToRobotJointIndex[] = {
+ PR2::ARM_R_PAN , PR2::ARM_R_SHOULDER_PITCH, PR2::ARM_R_SHOULDER_ROLL ,
+ PR2::ARM_R_ELBOW_PITCH , PR2::ARM_R_ELBOW_ROLL , PR2::ARM_R_WRIST_PITCH ,
+ PR2::ARM_R_WRIST_ROLL };
+ int headMapToRobotJointIndex[] = {
+ PR2::HEAD_YAW , PR2::HEAD_PITCH };
+ int laserScannerMapToRobotJointIndex[] = {
+ PR2::HEAD_LASER_PITCH };
+ int spineMapToRobotJointIndex[] = {
+ PR2::SPINE_ELEVATOR };
+ // hard coded mapping between jointcontroller array and hardware interface actuator array
+ int baseMapToHIActuatorIndex[] = {
+ PR2::CASTER_FL_STEER , PR2::CASTER_FL_DRIVE_L , PR2::CASTER_FL_DRIVE_R ,
+ PR2::CASTER_FR_STEER , PR2::CASTER_FR_DRIVE_L , PR2::CASTER_FR_DRIVE_R ,
+ PR2::CASTER_RL_STEER , PR2::CASTER_RL_DRIVE_L , PR2::CASTER_RL_DRIVE_R ,
+ PR2::CASTER_RR_STEER , PR2::CASTER_RR_DRIVE_L , PR2::CASTER_RR_DRIVE_R };
+ int leftArmMapToHIActuatorIndex[] = {
+ PR2::ARM_L_PAN , PR2::ARM_L_SHOULDER_PITCH, PR2::ARM_L_SHOULDER_ROLL ,
+ PR2::ARM_L_ELBOW_PITCH , PR2::ARM_L_ELBOW_ROLL , PR2::ARM_L_WRIST_PITCH ,
+ PR2::ARM_L_WRIST_ROLL };
+ int rightArmMapToHIActuatorIndex[] = {
+ PR2::ARM_R_PAN , PR2::ARM_R_SHOULDER_PITCH, PR2::ARM_R_SHOULDER_ROLL ,
+ PR2::ARM_R_ELBOW_PITCH , PR2::ARM_R_ELBOW_ROLL , PR2::ARM_R_WRIST_PITCH ,
+ PR2::ARM_R_WRIST_ROLL };
+ int headMapToHIActuatorIndex[] = {
+ PR2::HEAD_YAW , PR2::HEAD_PITCH };
+ int laserScannerMapToHIActuatorIndex[] = {
+ PR2::HEAD_LASER_PITCH };
+ int spineMapToHIActuatorIndex[] = {
+ PR2::SPINE_ELEVATOR };
+
+ int leftArmNumJoints = 7;
+ int rightArmNumJoints = 7;
+
+ baseController = new BaseController(r,"baseController"); // contro...
[truncated message content] |
|
From: <is...@us...> - 2008-07-25 01:29:20
|
Revision: 2112
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=2112&view=rev
Author: isucan
Date: 2008-07-25 01:29:28 +0000 (Fri, 25 Jul 2008)
Log Message:
-----------
motion planning without obstacles seems to be running....
Modified Paths:
--------------
pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp
pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/SpaceInformationKinematic.h
pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/extension/rrt/src/RRT.cpp
pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/src/SpaceInformationKinematic.cpp
pkg/trunk/robot_descriptions/wg_robot_description/pr2/pr2.xml
pkg/trunk/util/collision_space/include/collision_space/environmentODE.h
pkg/trunk/util/collision_space/src/collision_space/environmentODE.cpp
pkg/trunk/util/collision_space/src/test/test_kinematic_ode.cpp
Modified: pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp 2008-07-25 01:25:49 UTC (rev 2111)
+++ pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp 2008-07-25 01:29:28 UTC (rev 2112)
@@ -199,6 +199,7 @@
goal->state = new ompl::SpaceInformationKinematic::StateKinematic(dim);
for (int i = 0 ; i < dim ; ++i)
goal->state->values[i] = req.goal_state.vals[i];
+ goal->threshold = 1e-6;
p.si->setGoal(goal);
/* do the planning */
@@ -218,7 +219,7 @@
res.path.states[i].vals[j] = path->states[i]->values[j];
}
}
-
+
/* cleanup */
p.si->clearGoal();
p.si->clearStartStates(true);
Modified: pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/SpaceInformationKinematic.h
===================================================================
--- pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/SpaceInformationKinematic.h 2008-07-25 01:25:49 UTC (rev 2111)
+++ pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/SpaceInformationKinematic.h 2008-07-25 01:29:28 UTC (rev 2112)
@@ -145,7 +145,7 @@
m_isValidStateFnData = data;
}
- virtual void printState(const StateKinematic_t state, FILE* out = stdout);
+ virtual void printState(const StateKinematic_t state, FILE* out = stdout) const;
virtual void copyState(StateKinematic_t destination, const StateKinematic_t source)
{
memcpy(destination->values, source->values, sizeof(double) * m_stateDimension);
@@ -172,6 +172,8 @@
return m_isValidStateFn(state, m_isValidStateFnData);
}
+ virtual void printSettings(FILE *out = stdout) const;
+
protected:
unsigned int m_stateDimension;
Modified: pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/extension/rrt/src/RRT.cpp
===================================================================
--- pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/extension/rrt/src/RRT.cpp 2008-07-25 01:25:49 UTC (rev 2111)
+++ pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/extension/rrt/src/RRT.cpp 2008-07-25 01:29:28 UTC (rev 2112)
@@ -7,6 +7,9 @@
SpaceInformationKinematic::GoalStateKinematic_t goal_s = dynamic_cast<SpaceInformationKinematic::GoalStateKinematic_t>(si->getGoal());
unsigned int dim = si->getStateDimension();
+ printf("Received motion planning request for %f seconds.\n", solveTime);
+ si->printSettings();
+
ros::Time endTime = ros::Time::now() + ros::Duration(solveTime);
for (unsigned int i = 0 ; i < m_si->getStartStateCount() ; ++i)
@@ -63,7 +66,6 @@
if (si->checkMotion(nmotion->state, motion->state))
{
m_nn.add(motion);
-
if (goal_r->distanceGoal(motion->state) < goal_r->threshold)
{
solution = motion;
@@ -82,10 +84,14 @@
solution = solution->parent;
}
- /*set the solution path */
+ /* set the solution path */
SpaceInformationKinematic::PathKinematic_t path = new SpaceInformationKinematic::PathKinematic(m_si);
- for (int i = mpath.size() - 1 ; i >= 0 ; ++i)
- path->states.push_back(mpath[i]->state);
+ for (int i = mpath.size() - 1 ; i >= 0 ; --i)
+ {
+ SpaceInformationKinematic::StateKinematic_t st = new SpaceInformationKinematic::StateKinematic(dim);
+ si->copyState(st, mpath[i]->state);
+ path->states.push_back(st);
+ }
goal_r->setSolutionPath(path);
}
Modified: pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/src/SpaceInformationKinematic.cpp
===================================================================
--- pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/src/SpaceInformationKinematic.cpp 2008-07-25 01:25:49 UTC (rev 2111)
+++ pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/src/SpaceInformationKinematic.cpp 2008-07-25 01:29:28 UTC (rev 2112)
@@ -2,7 +2,7 @@
#include <algorithm>
#include <queue>
-void ompl::SpaceInformationKinematic::printState(const StateKinematic_t state, FILE* out)
+void ompl::SpaceInformationKinematic::printState(const StateKinematic_t state, FILE* out) const
{
for (unsigned int i = 0 ; i < m_stateDimension ; ++i)
fprintf(out, "%0.6f ", state->values[i]);
@@ -126,3 +126,17 @@
}
}
}
+
+void ompl::SpaceInformationKinematic::printSettings(FILE *out) const
+{
+ fprintf(out, "Kinematic state space settings:\n");
+ fprintf(out, " - dimension = %u\n", m_stateDimension);
+ fprintf(out, " - start states:\n");
+ for (unsigned int i = 0 ; i < getStartStateCount() ; ++i)
+ printState(dynamic_cast<const StateKinematic_t>(getStartState(i)), out);
+ fprintf(out, " - goal = %p\n", m_goal);
+ fprintf(out, " - bounding box:\n");
+ for (unsigned int i = 0 ; i < m_stateDimension ; ++i)
+ fprintf(out, "[%f, %f](%f) ", m_stateComponent[i].minValue, m_stateComponent[i].maxValue, m_stateComponent[i].resolution);
+ fprintf(out, "\n");
+}
Modified: pkg/trunk/robot_descriptions/wg_robot_description/pr2/pr2.xml
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description/pr2/pr2.xml 2008-07-25 01:25:49 UTC (rev 2111)
+++ pkg/trunk/robot_descriptions/wg_robot_description/pr2/pr2.xml 2008-07-25 01:29:28 UTC (rev 2112)
@@ -207,6 +207,7 @@
<axis> 1 0 0 </axis> <!-- direction of the joint in a global coordinate frame -->
<anchor>0 0 0 </anchor> <!-- point on the joint relative to the parent's anchor in a global coordinate frame -->
<calibration>1.5 -1 </calibration> <!-- Calibration stop/flag indicating location and then direction -->
+ <limit>-1 1</limit>
</define>
<define template="pr2_forearm_roll_inertial">
@@ -228,6 +229,7 @@
<axis> 0 1 0 </axis> <!-- direction of the joint in a global coordinate frame -->
<anchor>0 0 0 </anchor> <!-- point on the joint relative to the parent's anchor in a global coordinate frame -->
<calibration>1.5 -1 </calibration> <!-- Calibration stop/flag indicating location and then direction -->
+ <limit>-1 1</limit>
</define>
<define template="pr2_wrist_pitch_inertial">
@@ -250,6 +252,7 @@
<axis> 0 1 0 </axis> <!-- direction of the joint in a global coordinate frame -->
<anchor>0 0 0 </anchor> <!-- point on the joint relative to the parent's anchor in a global coordinate frame -->
<calibration>1.5 -1 </calibration> <!-- Calibration stop/flag indicating location and then direction -->
+ <limit>-1 1</limit>
</define>
<define template="pr2_wrist_roll_inertial">
Modified: pkg/trunk/util/collision_space/include/collision_space/environmentODE.h
===================================================================
--- pkg/trunk/util/collision_space/include/collision_space/environmentODE.h 2008-07-25 01:25:49 UTC (rev 2111)
+++ pkg/trunk/util/collision_space/include/collision_space/environmentODE.h 2008-07-25 01:29:28 UTC (rev 2112)
@@ -67,6 +67,7 @@
}
dSpaceID getODESpace(void) const;
+ dSpaceID getModelODESpace(unsigned int model_id) const;
/** Check if a model is in collision */
virtual bool isCollision(unsigned int model_id);
Modified: pkg/trunk/util/collision_space/src/collision_space/environmentODE.cpp
===================================================================
--- pkg/trunk/util/collision_space/src/collision_space/environmentODE.cpp 2008-07-25 01:25:49 UTC (rev 2111)
+++ pkg/trunk/util/collision_space/src/collision_space/environmentODE.cpp 2008-07-25 01:29:28 UTC (rev 2112)
@@ -173,6 +173,12 @@
return m_space;
}
+dSpaceID collision_space::EnvironmentModelODE::getModelODESpace(unsigned int model_id) const
+{
+ return m_kgeoms[model_id].s;
+}
+
+
struct CollisionData
{
bool collides;
Modified: pkg/trunk/util/collision_space/src/test/test_kinematic_ode.cpp
===================================================================
--- pkg/trunk/util/collision_space/src/test/test_kinematic_ode.cpp 2008-07-25 01:25:49 UTC (rev 2111)
+++ pkg/trunk/util/collision_space/src/test/test_kinematic_ode.cpp 2008-07-25 01:29:28 UTC (rev 2112)
@@ -146,8 +146,6 @@
int main(int argc, char **argv)
{
- dInitODE();
-
robot_desc::URDF model;
model.loadFile("/u/isucan/ros/ros-pkg/robot_descriptions/wg_robot_description/pr2/pr2.xml");
@@ -169,7 +167,7 @@
m->computeTransforms(param, m->getGroupID("pr2::leftArm"));
km->updateRobotModel(0);
- robotSpace = dynamic_cast<EnvironmentModelODE*>(km)->getODESpace();
+ robotSpace = dynamic_cast<EnvironmentModelODE*>(km)->getModelODESpace(0);
dsFunctions fn;
fn.version = DS_VERSION;
@@ -184,7 +182,6 @@
delete km;
delete[] param;
- dCloseODE();
return 0;
}
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <MP...@us...> - 2008-07-25 01:58:08
|
Revision: 2117
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=2117&view=rev
Author: MPPics
Date: 2008-07-25 01:58:15 +0000 (Fri, 25 Jul 2008)
Log Message:
-----------
Added features to wx camera panel and small fixes to rosGazeboNode.
Modified Paths:
--------------
pkg/trunk/drivers/robot/pr2/pr2Core/include/pr2Core/pr2Core.h
pkg/trunk/simulators/rosgazebo/src/RosGazeboNode.cpp
pkg/trunk/visualization/pr2_gui/src/launcherimpl.cpp
pkg/trunk/visualization/wx_ros/wx_camera_panel/camera_panels.fbp
pkg/trunk/visualization/wx_ros/wx_camera_panel/src/wx_camera_panel/CameraPanel.cpp
pkg/trunk/visualization/wx_ros/wx_camera_panel/src/wx_camera_panel/CameraPanel.h
pkg/trunk/visualization/wx_ros/wx_camera_panel/src/wx_camera_panel/CameraPanelsGenerated.cpp
pkg/trunk/visualization/wx_ros/wx_camera_panel/src/wx_camera_panel/CameraPanelsGenerated.h
pkg/trunk/visualization/wx_ros/wx_camera_panel/src/wx_camera_panel/CameraSetupDialog.cpp
pkg/trunk/visualization/wx_ros/wx_camera_panel/src/wx_camera_panel/CameraSetupDialog.h
Modified: pkg/trunk/drivers/robot/pr2/pr2Core/include/pr2Core/pr2Core.h
===================================================================
--- pkg/trunk/drivers/robot/pr2/pr2Core/include/pr2Core/pr2Core.h 2008-07-25 01:44:46 UTC (rev 2116)
+++ pkg/trunk/drivers/robot/pr2/pr2Core/include/pr2Core/pr2Core.h 2008-07-25 01:58:15 UTC (rev 2117)
@@ -345,7 +345,7 @@
const point3 BASE_LEFT_ARM_OFFSET = {-.1829361, 0.1329361, 0.80981 };
const point3 BASE_RIGHT_ARM_OFFSET = {-.1829361, -0.1329361, 0.80981 };
- const point3 HEAD_PAN_HEAD_PITCH_OFFSET = {0,0,0};
+ const point3 HEAD_PAN_HEAD_PITCH_OFFSET = {.06,0,.08};
const point3 TORSO_HEAD_OFFSET = {-.02, 0, 0.80981};
const point3 TORSO_TILT_LASER_OFFSET = {.07, 0, .68};
Modified: pkg/trunk/simulators/rosgazebo/src/RosGazeboNode.cpp
===================================================================
--- pkg/trunk/simulators/rosgazebo/src/RosGazeboNode.cpp 2008-07-25 01:44:46 UTC (rev 2116)
+++ pkg/trunk/simulators/rosgazebo/src/RosGazeboNode.cpp 2008-07-25 01:58:15 UTC (rev 2117)
@@ -324,7 +324,7 @@
//advertise<std_msgs::LaserScan>("laser");
advertise<std_msgs::LaserScan>("scan");
advertise<std_msgs::RobotBase2DOdom>("odom");
- advertise<std_msgs::Image>("PTZR_image");
+ //advertise<std_msgs::Image>("image");
advertise<std_msgs::Image>("image");
advertise<std_msgs::Image>("image_ptz_right");
advertise<std_msgs::Image>("image_ptz_left");
@@ -616,7 +616,7 @@
this->img_ptz_right.data = buf_ptz_right;
//memcpy(this->img_ptz_right.data,buf,data_size);
- publish("image",this->img_ptz_right);
+ //publish("image",this->img_ptz_right);
publish("image_ptz_right",this->img_ptz_right);
}
}
@@ -1094,13 +1094,13 @@
// FIXME: not implemented
tf.sendEuler("FRAMEID_HEAD_TILT_BASE",
- "FRAMEID_TORSO",
+ "FRAMEID_HEAD_PAN_BASE",
+ PR2::HEAD_PAN_HEAD_PITCH_OFFSET.x,
0.0,
+ PR2::HEAD_PAN_HEAD_PITCH_OFFSET.z,
0.0,
- 1.05,
0.0,
0.0,
- 0.0,
odomMsg.header.stamp);
// FIXME: not implemented
Modified: pkg/trunk/visualization/pr2_gui/src/launcherimpl.cpp
===================================================================
--- pkg/trunk/visualization/pr2_gui/src/launcherimpl.cpp 2008-07-25 01:44:46 UTC (rev 2116)
+++ pkg/trunk/visualization/pr2_gui/src/launcherimpl.cpp 2008-07-25 01:58:15 UTC (rev 2117)
@@ -263,7 +263,7 @@
tiltPTZL_S->Enable(true);
zoomPTZL_S->Enable(true);
PTZL_B->Enable(true);
- myNode->subscribe("PTZL_image", PTZLImage, &LauncherImpl::incomingPTZLImageConn,this);
+ myNode->subscribe("image_ptz_left", PTZLImage, &LauncherImpl::incomingPTZLImageConn,this);
myNode->subscribe("PTZL_state", PTZL_state, &LauncherImpl::incomingPTZLState,this);
myNode->advertise<std_msgs::PTZActuatorCmd>("PTZL_cmd");
@@ -273,7 +273,7 @@
{
consoleOut(wxT("Closing Left Pan-Tilt-Zoom\n"));
LeftDock_FGS->Hide(PTZL_SBS,true);
- myNode->unsubscribe("PTZL_image");
+ myNode->unsubscribe("image_ptz_left");
panPTZL_S->Enable(false);
tiltPTZL_S->Enable(false);
zoomPTZL_S->Enable(false);
@@ -371,7 +371,7 @@
tiltPTZR_S->Enable(true);
zoomPTZR_S->Enable(true);
PTZR_B->Enable(true);
- myNode->subscribe("PTZR_image", PTZRImage, &LauncherImpl::incomingPTZRImageConn,this);
+ myNode->subscribe("image_ptz_right", PTZRImage, &LauncherImpl::incomingPTZRImageConn,this);
myNode->subscribe("PTZR_state", PTZR_state, &LauncherImpl::incomingPTZRState,this);
myNode->advertise<std_msgs::PTZActuatorCmd>("PTZR_cmd");
//*PTZR_bmp = NULL;
@@ -380,7 +380,7 @@
{
consoleOut(wxT("Closing Right Pan-Tilt-Zoom\n"));
RightDock_FGS->Hide(PTZR_SBS,true);
- myNode->unsubscribe("PTZR_image");
+ myNode->unsubscribe("image_ptz_right");
panPTZR_S->Enable(false);
tiltPTZR_S->Enable(false);
zoomPTZR_S->Enable(false);
@@ -480,14 +480,14 @@
LeftDock_FGS->Show(WristL_SBS,true);
Layout();
Fit();
- myNode->subscribe("WristL_image", WristLImage, &LauncherImpl::incomingWristLImageConn,this);
+ myNode->subscribe("image_wrist_left", WristLImage, &LauncherImpl::incomingWristLImageConn,this);
//*WristL_bmp = NULL;
}
else
{
consoleOut(wxT("Closing Left Wrist\n"));
LeftDock_FGS->Hide(WristL_SBS,true);
- myNode->unsubscribe("WristL_image");
+ myNode->unsubscribe("image_wrist_left");
wxSize size(0,0);
WristL_B->SetMinSize(size);
Layout();
@@ -551,14 +551,14 @@
RightDock_FGS->Show(WristR_SBS,true);
Layout();
Fit();
- myNode->subscribe("WristR_image", WristRImage, &LauncherImpl::incomingWristRImageConn,this);
+ myNode->subscribe("image_wrist_right", WristRImage, &LauncherImpl::incomingWristRImageConn,this);
//*WristR_bmp = NULL;
}
else
{
consoleOut(wxT("Closing Right Wrist\n"));
RightDock_FGS->Hide(WristR_SBS,true);
- myNode->unsubscribe("WristR_image");
+ myNode->unsubscribe("image_wrist_right");
wxSize size(0,0);
WristR_B->SetMinSize(size);
Layout();
Modified: pkg/trunk/visualization/wx_ros/wx_camera_panel/camera_panels.fbp
===================================================================
--- pkg/trunk/visualization/wx_ros/wx_camera_panel/camera_panels.fbp 2008-07-25 01:44:46 UTC (rev 2116)
+++ pkg/trunk/visualization/wx_ros/wx_camera_panel/camera_panels.fbp 2008-07-25 01:58:15 UTC (rev 2117)
@@ -269,7 +269,7 @@
<property name="minimum_size"></property>
<property name="name">CameraSetupDialogBase</property>
<property name="pos"></property>
- <property name="size">472,231</property>
+ <property name="size">472,400</property>
<property name="style">wxDEFAULT_DIALOG_STYLE</property>
<property name="subclass"></property>
<property name="title">Camera Setup</property>
@@ -335,7 +335,7 @@
<property name="permission">none</property>
<object class="sizeritem" expanded="1">
<property name="border">5</property>
- <property name="flag">wxALL</property>
+ <property name="flag">wxALL|wxALIGN_CENTER_VERTICAL</property>
<property name="proportion">1</property>
<object class="wxStaticText" expanded="1">
<property name="bg"></property>
@@ -395,7 +395,7 @@
<property name="permission">none</property>
<object class="sizeritem" expanded="1">
<property name="border">5</property>
- <property name="flag">wxALL</property>
+ <property name="flag">wxALL|wxALIGN_CENTER_VERTICAL</property>
<property name="proportion">1</property>
<object class="wxTextCtrl" expanded="1">
<property name="bg"></property>
@@ -450,7 +450,7 @@
</object>
<object class="sizeritem" expanded="1">
<property name="border">5</property>
- <property name="flag">wxALL</property>
+ <property name="flag">wxALL|wxALIGN_CENTER_VERTICAL</property>
<property name="proportion">0</property>
<object class="wxButton" expanded="1">
<property name="bg"></property>
@@ -520,16 +520,890 @@
<event name="OnUpdateUI"></event>
<object class="sizeritem" expanded="1">
<property name="border">5</property>
+ <property name="flag">wxALL</property>
+ <property name="proportion">0</property>
+ <object class="wxCheckBox" expanded="1">
+ <property name="bg"></property>
+ <property name="checked">1</property>
+ <property name="context_help"></property>
+ <property name="enabled">1</property>
+ <property name="fg"></property>
+ <property name="font"></property>
+ <property name="hidden">0</property>
+ <property name="id">wxID_ANY</property>
+ <property name="label">Enable</property>
+ <property name="maximum_size"></property>
+ <property name="minimum_size"></property>
+ <property name="name">m_EnablePTZCheck</property>
+ <property name="permission">protected</property>
+ <property name="pos"></property>
+ <property name="size"></property>
+ <property name="style"></property>
+ <property name="subclass"></property>
+ <property name="tooltip"></property>
+ <property name="window_extra_style"></property>
+ <property name="window_name"></property>
+ <property name="window_style"></property>
+ <event name="OnChar"></event>
+ <event name="OnCheckBox">OnPTZEnableChecked</event>
+ <event name="OnEnterWindow"></event>
+ <event name="OnEraseBackground"></event>
+ <event name="OnKeyDown"></event>
+ <event name="OnKeyUp"></event>
+ <event name="OnKillFocus"></event>
+ <event name="OnLeaveWindow"></event>
+ <event name="OnLeftDClick"></event>
+ <event name="OnLeftDown"></event>
+ <event name="OnLeftUp"></event>
+ <event name="OnMiddleDClick"></event>
+ <event name="OnMiddleDown"></event>
+ <event name="OnMiddleUp"></event>
+ <event name="OnMotion"></event>
+ <event name="OnMouseEvents"></event>
+ <event name="OnMouseWheel"></event>
+ <event name="OnPaint"></event>
+ <event name="OnRightDClick"></event>
+ <event name="OnRightDown"></event>
+ <event name="OnRightUp"></event>
+ <event name="OnSetFocus"></event>
+ <event name="OnSize"></event>
+ <event name="OnUpdateUI"></event>
+ </object>
+ </object>
+ <object class="sizeritem" expanded="1">
+ <property name="border">5</property>
<property name="flag">wxEXPAND</property>
<property name="proportion">1</property>
- <object class="wxBoxSizer" expanded="1">
+ <object class="wxBoxSizer" expanded="0">
<property name="minimum_size"></property>
+ <property name="name">bSizer12</property>
+ <property name="orient">wxHORIZONTAL</property>
+ <property name="permission">none</property>
+ <object class="sizeritem" expanded="1">
+ <property name="border">5</property>
+ <property name="flag">wxALL|wxALIGN_CENTER_VERTICAL</property>
+ <property name="proportion">1</property>
+ <object class="wxStaticText" expanded="1">
+ <property name="bg"></property>
+ <property name="context_help"></property>
+ <property name="enabled">1</property>
+ <property name="fg"></property>
+ <property name="font"></property>
+ <property name="hidden">0</property>
+ <property name="id">wxID_ANY</property>
+ <property name="label">Pan Limits:</property>
+ <property name="maximum_size"></property>
+ <property name="minimum_size"></property>
+ <property name="name">m_staticText4</property>
+ <property name="permission">protected</property>
+ <property name="pos"></property>
+ <property name="size"></property>
+ <property name="style"></property>
+ <property name="subclass"></property>
+ <property name="tooltip"></property>
+ <property name="window_extra_style"></property>
+ <property name="window_name"></property>
+ <property name="window_style"></property>
+ <property name="wrap">-1</property>
+ <event name="OnChar"></event>
+ <event name="OnEnterWindow"></event>
+ <event name="OnEraseBackground"></event>
+ <event name="OnKeyDown"></event>
+ <event name="OnKeyUp"></event>
+ <event name="OnKillFocus"></event>
+ <event name="OnLeaveWindow"></event>
+ <event name="OnLeftDClick"></event>
+ <event name="OnLeftDown"></event>
+ <event name="OnLeftUp"></event>
+ <event name="OnMiddleDClick"></event>
+ <event name="OnMiddleDown"></event>
+ <event name="OnMiddleUp"></event>
+ <event name="OnMotion"></event>
+ <event name="OnMouseEvents"></event>
+ <event name="OnMouseWheel"></event>
+ <event name="OnPaint"></event>
+ <event name="OnRightDClick"></event>
+ <event name="OnRightDown"></event>
+ <event name="OnRightUp"></event>
+ <event name="OnSetFocus"></event>
+ <event name="OnSize"></event>
+ <event name="OnUpdateUI"></event>
+ </object>
+ </object>
+ <object class="sizeritem" expanded="1">
+ <property name="border">5</property>
+ <property name="flag">wxALL|wxALIGN_CENTER_VERTICAL</property>
+ <property name="proportion">0</property>
+ <object class="wxStaticText" expanded="1">
+ <property name="bg"></property>
+ <property name="context_help"></property>
+ <property name="enabled">1</property>
+ <property name="fg"></property>
+ <property name="font"></property>
+ <property name="hidden">0</property>
+ <property name="id">wxID_ANY</property>
+ <property name="label">Min:</property>
+ <property name="maximum_size"></property>
+ <property name="minimum_size"></property>
+ <property name="name">m_staticText5</property>
+ <property name="permission">protected</property>
+ <property name="pos"></property>
+ <property name="size"></property>
+ <property name="style"></property>
+ <property name="subclass"></property>
+ <property name="tooltip"></property>
+ <property name="window_extra_style"></property>
+ <property name="window_name"></property>
+ <property name="window_style"></property>
+ <property name="wrap">-1</property>
+ <event name="OnChar"></event>
+ <event name="OnEnterWindow"></event>
+ <event name="OnEraseBackground"></event>
+ <event name="OnKeyDown"></event>
+ <event name="OnKeyUp"></event>
+ <event name="OnKillFocus"></event>
+ <event name="OnLeaveWindow"></event>
+ <event name="OnLeftDClick"></event>
+ <event name="OnLeftDown"></event>
+ <event name="OnLeftUp"></event>
+ <event name="OnMiddleDClick"></event>
+ <event name="OnMiddleDown"></event>
+ <event name="OnMiddleUp"></event>
+ <event name="OnMotion"></event>
+ <event name="OnMouseEvents"></event>
+ <event name="OnMouseWheel"></event>
+ <event name="OnPaint"></event>
+ <event name="OnRightDClick"></event>
+ <event name="OnRightDown"></event>
+ <event name="OnRightUp"></event>
+ <event name="OnSetFocus"></event>
+ <event name="OnSize"></event>
+ <event name="OnUpdateUI"></event>
+ </object>
+ </object>
+ <object class="sizeritem" expanded="1">
+ <property name="border">5</property>
+ <property name="flag">wxALL|wxALIGN_CENTER_VERTICAL</property>
+ <property name="proportion">0</property>
+ <object class="wxSpinCtrl" expanded="1">
+ <property name="bg"></property>
+ <property name="context_help"></property>
+ <property name="enabled">1</property>
+ <property name="fg"></property>
+ <property name="font"></property>
+ <property name="hidden">0</property>
+ <property name="id">wxID_ANY</property>
+ <property name="initial">-169</property>
+ <property name="max">9999999</property>
+ <property name="maximum_size"></property>
+ <property name="min">-9999999</property>
+ <property name="minimum_size"></property>
+ <property name="name">m_PanMinSpin</property>
+ <property name="permission">protected</property>
+ <property name="pos"></property>
+ <property name="size"></property>
+ <property name="style">wxSP_ARROW_KEYS</property>
+ <property name="subclass"></property>
+ <property name="tooltip"></property>
+ <property name="value"></property>
+ <property name="window_extra_style"></property>
+ <property name="window_name"></property>
+ <property name="window_style"></property>
+ <event name="OnChar"></event>
+ <event name="OnEnterWindow"></event>
+ <event name="OnEraseBackground"></event>
+ <event name="OnKeyDown"></event>
+ <event name="OnKeyUp"></event>
+ <event name="OnKillFocus"></event>
+ <event name="OnLeaveWindow"></event>
+ <event name="OnLeftDClick"></event>
+ <event name="OnLeftDown"></event>
+ <event name="OnLeftUp"></event>
+ <event name="OnMiddleDClick"></event>
+ <event name="OnMiddleDown"></event>
+ <event name="OnMiddleUp"></event>
+ <event name="OnMotion"></event>
+ <event name="OnMouseEvents"></event>
+ <event name="OnMouseWheel"></event>
+ <event name="OnPaint"></event>
+ <event name="OnRightDClick"></event>
+ <event name="OnRightDown"></event>
+ <event name="OnRightUp"></event>
+ <event name="OnSetFocus"></event>
+ <event name="OnSize"></event>
+ <event name="OnSpinCtrl"></event>
+ <event name="OnSpinCtrlText"></event>
+ <event name="OnUpdateUI"></event>
+ </object>
+ </object>
+ <object class="sizeritem" expanded="1">
+ <property name="border">5</property>
+ <property name="flag">wxALL|wxALIGN_CENTER_VERTICAL</property>
+ <property name="proportion">0</property>
+ <object class="wxStaticText" expanded="1">
+ <property name="bg"></property>
+ <property name="context_help"></property>
+ <property name="enabled">1</property>
+ <property name="fg"></property>
+ <property name="font"></property>
+ <property name="hidden">0</property>
+ <property name="id">wxID_ANY</property>
+ <property name="label">Max:</property>
+ <property name="maximum_size"></property>
+ <property name="minimum_size"></property>
+ <property name="name">m_staticText6</property>
+ <property name="permission">protected</property>
+ <property name="pos"></property>
+ <property name="size"></property>
+ <property name="style"></property>
+ <property name="subclass"></property>
+ <property name="tooltip"></property>
+ <property name="window_extra_style"></property>
+ <property name="window_name"></property>
+ <property name="window_style"></property>
+ <property name="wrap">-1</property>
+ <event name="OnChar"></event>
+ <event name="OnEnterWindow"></event>
+ <event name="OnEraseBackground"></event>
+ <event name="OnKeyDown"></event>
+ <event name="OnKeyUp"></event>
+ <event name="OnKillFocus"></event>
+ <event name="OnLeaveWindow"></event>
+ <event name="OnLeftDClick"></event>
+ <event name="OnLeftDown"></event>
+ <event name="OnLeftUp"></event>
+ <event name="OnMiddleDClick"></event>
+ <event name="OnMiddleDown"></event>
+ <event name="OnMiddleUp"></event>
+ <event name="OnMotion"></event>
+ <event name="OnMouseEvents"></event>
+ <event name="OnMouseWheel"></event>
+ <event name="OnPaint"></event>
+ <event name="OnRightDClick"></event>
+ <event name="OnRightDown"></event>
+ <event name="OnRightUp"></event>
+ <event name="OnSetFocus"></event>
+ <event name="OnSize"></event>
+ <event name="OnUpdateUI"></event>
+ </object>
+ </object>
+ <object class="sizeritem" expanded="1">
+ <property name="border">5</property>
+ <property name="flag">wxALL|wxALIGN_CENTER_VERTICAL</property>
+ <property name="proportion">0</property>
+ <object class="wxSpinCtrl" expanded="1">
+ <property name="bg"></property>
+ <property name="context_help"></property>
+ <property name="enabled">1</property>
+ <property name="fg"></property>
+ <property name="font"></property>
+ <property name="hidden">0</property>
+ ...
[truncated message content] |
|
From: <rob...@us...> - 2008-07-25 06:44:25
|
Revision: 2124
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=2124&view=rev
Author: rob_wheeler
Date: 2008-07-25 06:44:34 +0000 (Fri, 25 Jul 2008)
Log Message:
-----------
Add current time to HardwareInterface
Modified Paths:
--------------
pkg/trunk/mechanism/hw_interface/include/hw_interface/hardware_interface.h
pkg/trunk/robot_control_loops/pr2_etherCAT/src/main.cpp
Modified: pkg/trunk/mechanism/hw_interface/include/hw_interface/hardware_interface.h
===================================================================
--- pkg/trunk/mechanism/hw_interface/include/hw_interface/hardware_interface.h 2008-07-25 03:55:16 UTC (rev 2123)
+++ pkg/trunk/mechanism/hw_interface/include/hw_interface/hardware_interface.h 2008-07-25 06:44:34 UTC (rev 2124)
@@ -84,6 +84,7 @@
}
Actuator *actuator;
int numActuators;
+ double current_time_;
};
#endif
Modified: pkg/trunk/robot_control_loops/pr2_etherCAT/src/main.cpp
===================================================================
--- pkg/trunk/robot_control_loops/pr2_etherCAT/src/main.cpp 2008-07-25 03:55:16 UTC (rev 2123)
+++ pkg/trunk/robot_control_loops/pr2_etherCAT/src/main.cpp 2008-07-25 06:44:34 UTC (rev 2124)
@@ -55,7 +55,7 @@
// Switch to hard real-time
int period = 1000000;
- struct timespec tick;
+ struct timespec tick, now;
#if defined(__XENO__)
pthread_set_mode_np(0, PTHREAD_PRIMARY|PTHREAD_WARNSW);
#endif
@@ -64,6 +64,8 @@
while (!quit)
{
ec.update();
+ clock_gettime(CLOCK_REALTIME, &now);
+ ec.hw->current_time_ = now.tv_nsec / NSEC_PER_SEC + now.tv_sec;
mc.update();
tick.tv_nsec += period;
while (tick.tv_nsec >= NSEC_PER_SEC)
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <hsu...@us...> - 2008-07-25 08:09:12
|
Revision: 2128
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=2128&view=rev
Author: hsujohnhsu
Date: 2008-07-25 08:09:21 +0000 (Fri, 25 Jul 2008)
Log Message:
-----------
added laser ros plugin stub for gazebo.
Modified Paths:
--------------
pkg/trunk/3rdparty/gazebo/manifest.xml
pkg/trunk/drivers/simulator/gazebo_plugin/CMakeLists.txt
Added Paths:
-----------
pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/Ros_Laser.hh
pkg/trunk/drivers/simulator/gazebo_plugin/src/Ros_Laser.cc
Modified: pkg/trunk/3rdparty/gazebo/manifest.xml
===================================================================
--- pkg/trunk/3rdparty/gazebo/manifest.xml 2008-07-25 08:04:35 UTC (rev 2127)
+++ pkg/trunk/3rdparty/gazebo/manifest.xml 2008-07-25 08:09:21 UTC (rev 2128)
@@ -11,7 +11,7 @@
<depend package="opende" />
<depend package="ogre" />
<export>
- <cpp lflags="-Wl,-rpath,${prefix}/gazebo/lib -L${prefix}/gazebo/lib -lgazebo -lgazeboServer" cflags="-I${prefix}/gazebo/include -I${prefix}/gazebo-svn/server/sensors -I${prefix}/gazebo-svn/server/sensors/camera -I${prefix}/gazebo-svn/server -I${prefix}/gazebo-svn/server/rendering"/>
+ <cpp lflags="-Wl,-rpath,${prefix}/gazebo/lib -L${prefix}/gazebo/lib -lgazebo -lgazeboServer" cflags="-I${prefix}/gazebo/include -I${prefix}/gazebo-svn/server/physics -I${prefix}/gazebo-svn/server/sensors -I${prefix}/gazebo-svn/server/sensors/ray -I${prefix}/gazebo-svn/server/sensors/camera -I${prefix}/gazebo-svn/server -I${prefix}/gazebo-svn/server/rendering"/>
</export>
</package>
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/CMakeLists.txt
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/CMakeLists.txt 2008-07-25 08:04:35 UTC (rev 2127)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/CMakeLists.txt 2008-07-25 08:09:21 UTC (rev 2128)
@@ -4,6 +4,7 @@
rospack_add_library(Pr2_Actarray src/Pr2_Actarray.cc)
rospack_add_library(Pr2_Gripper src/Pr2_Gripper.cc)
rospack_add_library(Ros_Camera src/Ros_Camera.cc)
+rospack_add_library(Ros_Laser src/Ros_Laser.cc)
rospack_add_library(Ros_Node src/Ros_Node.cc)
rospack_add_library(P3D src/P3D.cc)
Added: pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/Ros_Laser.hh
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/Ros_Laser.hh (rev 0)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/Ros_Laser.hh 2008-07-25 08:09:21 UTC (rev 2128)
@@ -0,0 +1,128 @@
+/*
+ * 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: ros laser controller.
+ * Author: Nathan Koenig
+ * Date: 01 Feb 2007
+ * SVN: $Id: Ros_Laser.hh 6656 2008-06-20 22:52:19Z natepak $
+ */
+
+#ifndef ROS_LASER_HH
+#define ROS_LASER_HH
+
+#include <gazebo/Controller.hh>
+
+#include <ros/node.h>
+#include <std_msgs/Image.h>
+
+namespace gazebo
+{
+ class LaserIface;
+ class FiducialIface;
+ class RaySensor;
+
+/// @addtogroup gazebo_controller
+/// @{
+/** \defgroup ros ros
+
+ \brief ros laser controller.
+
+ This is a controller that collects data from a ray sensor, and populates a libgazebo laser interface.
+
+ \verbatim
+ <model:physical name="laser_model">
+ <body:box name="laser_body">
+
+ <sensor:ray name="laser">
+ <controller:ros_laser name="controller-name">
+ <interface:laser name="iface-name"/>
+ </controller:ros_laser>
+ </sensor:ray>
+
+ </body:box>
+ </model:physical>
+ \endverbatim
+
+\{
+*/
+
+/// \brief ros laser controller.
+///
+/// This is a controller that simulates a ros laser
+class Ros_Laser : public Controller
+{
+ /// \brief Constructor
+ /// \param parent The parent entity, must be a Model or a Sensor
+ public: Ros_Laser(Entity *parent);
+
+ /// \brief Destructor
+ public: virtual ~Ros_Laser();
+
+ /// \brief Load the controller
+ /// \param node XML config node
+ /// \return 0 on success
+ protected: virtual void LoadChild(XMLConfigNode *node);
+
+ /// \brief Init the controller
+ /// \return 0 on success
+ protected: virtual void InitChild();
+
+ /// \brief Update the controller
+ /// \return 0 on success
+ protected: virtual void UpdateChild();
+
+ /// \brief Finalize the controller
+ /// \return 0 on success
+ protected: virtual void FiniChild();
+
+ /// \brief Put laser data to the iface
+ private: void PutLaserData();
+
+ /// \brief Put fiducial data to the iface
+ private: void PutFiducialData();
+
+ /// The laser interface
+ private: LaserIface *laserIface;
+
+ private: FiducialIface *fiducialIface;
+
+ /// The parent sensor
+ private: RaySensor *myParent;
+
+ // pointer to ros node
+ private: ros::node *rosnode;
+
+ // ros message
+ private: std_msgs::Image image;
+
+ // topic name
+ private: std::string topicName;
+
+
+};
+
+/** /} */
+/// @}
+
+}
+
+#endif
+
Added: pkg/trunk/drivers/simulator/gazebo_plugin/src/Ros_Laser.cc
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/src/Ros_Laser.cc (rev 0)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/src/Ros_Laser.cc 2008-07-25 08:09:21 UTC (rev 2128)
@@ -0,0 +1,317 @@
+/*
+ * 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: Ros Laser controller.
+ * Author: Nathan Koenig
+ * Date: 01 Feb 2007
+ * SVN info: $Id: Ros_Laser.cc 6683 2008-06-25 19:12:30Z natepak $
+ */
+
+#include <algorithm>
+#include <assert.h>
+
+#include <gazebo/Sensor.hh>
+#include <gazebo/Global.hh>
+#include <gazebo/XMLConfig.hh>
+#include <gazebo/HingeJoint.hh>
+#include <gazebo/World.hh>
+#include <gazebo/Simulator.hh>
+#include <gazebo/gazebo.h>
+#include <gazebo/GazeboError.hh>
+#include <gazebo/ControllerFactory.hh>
+#include "RaySensor.hh"
+#include <gazebo_plugin/Ros_Laser.hh>
+
+using namespace gazebo;
+
+GZ_REGISTER_DYNAMIC_CONTROLLER("ros_laser", Ros_Laser);
+
+////////////////////////////////////////////////////////////////////////////////
+// Constructor
+Ros_Laser::Ros_Laser(Entity *parent)
+ : Controller(parent)
+{
+ this->myParent = dynamic_cast<RaySensor*>(this->parent);
+
+ if (!this->myParent)
+ gzthrow("Ros_Laser controller requires a Ray Sensor as its parent");
+
+ this->laserIface = NULL;
+ this->fiducialIface = NULL;
+
+ rosnode = ros::g_node; // comes from where? common.h exports as global variable
+ int argc;
+ char** argv = NULL;
+ if (rosnode == NULL)
+ {
+ // start a ros node if none exist
+ ros::init(argc,argv);
+ rosnode = new ros::node("ros_gazebo");
+ printf("-------------------- starting node \n");
+ }
+
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// Destructor
+Ros_Laser::~Ros_Laser()
+{
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// Load the controller
+void Ros_Laser::LoadChild(XMLConfigNode *node)
+{
+ std::vector<Iface*>::iterator iter;
+
+ for (iter = this->ifaces.begin(); iter != this->ifaces.end(); iter++)
+ {
+ if ((*iter)->GetType() == "laser")
+ this->laserIface = dynamic_cast<LaserIface*>(*iter);
+ else if ((*iter)->GetType() == "fiducial")
+ this->fiducialIface = dynamic_cast<FiducialIface*>(*iter);
+ }
+
+ if (!this->laserIface) gzthrow("Ros_Laser controller requires a LaserIface");
+
+ this->topicName = node->GetString("topicName","default_ros_laser",0); //read from xml file
+
+ std::cout << "================= " << this->topicName << std::endl;
+ rosnode->advertise<std_msgs::Image>(this->topicName);
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// Initialize the controller
+void Ros_Laser::InitChild()
+{
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// Update the controller
+void Ros_Laser::UpdateChild()
+{
+ bool laserOpened = false;
+ bool fidOpened = false;
+
+ if (this->laserIface->Lock(1))
+ {
+ laserOpened = this->laserIface->GetOpenCount() > 0;
+ //std::cout << " laser open count " << this->laserIface->GetOpenCount() << std::endl;
+ this->laserIface->Unlock();
+ }
+
+ if (this->fiducialIface && this->fiducialIface->Lock(1))
+ {
+ fidOpened = this->fiducialIface->GetOpenCount() > 0;
+ this->fiducialIface->Unlock();
+ }
+
+ if (laserOpened)
+ {
+ this->myParent->SetActive(true);
+ this->PutLaserData();
+ }
+
+ if (fidOpened)
+ {
+ this->myParent->SetActive(true);
+ this->PutFiducialData();
+ }
+
+ if (!laserOpened && !fidOpened)
+ {
+ this->myParent->SetActive(false);
+ }
+ //std::cout << " active " << this->myParent->IsActive() << std::endl;
+
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// Finalize the controller
+void Ros_Laser::FiniChild()
+{
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// Put laser data to the interface
+void Ros_Laser::PutLaserData()
+{
+ int i, ja, jb;
+ double ra, rb, r, b;
+ int v;
+
+ double maxAngle = this->myParent->GetMaxAngle();
+ double minAngle = this->myParent->GetMinAngle();
+
+ double maxRange = this->myParent->GetMaxRange();
+ double minRange = this->myParent->GetMinRange();
+ int rayCount = this->myParent->GetRayCount();
+ int rangeCount = this->myParent->GetRangeCount();
+
+ if (this->laserIface->Lock(1))
+ {
+ // Data timestamp
+ this->laserIface->data->head.time = Simulator::Instance()->GetSimTime();
+
+ // Read out the laser range data
+ this->laserIface->data->min_angle = minAngle;
+ this->laserIface->data->max_angle = maxAngle;
+ this->laserIface->data->res_angle = (maxAngle - minAngle) / (rangeCount - 1);
+ this->laserIface->data->res_range = 0.1;
+ this->laserIface->data->max_range = maxRange;
+ this->laserIface->data->range_count = rangeCount;
+
+ assert(this->laserIface->data->range_count < GZ_LASER_MAX_RANGES );
+
+ // Interpolate the range readings from the rays
+ for (i = 0; i<rangeCount; i++)
+ {
+ b = (double) i * (rayCount - 1) / (rangeCount - 1);
+ ja = (int) floor(b);
+ jb = std::min(ja + 1, rayCount - 1);
+ b = b - floor(b);
+
+ assert(ja >= 0 && ja < rayCount);
+ assert(jb >= 0 && jb < rayCount);
+
+ ra = std::min(this->myParent->GetRange(ja) , maxRange);
+ rb = std::min(this->myParent->GetRange(jb) , maxRange);
+
+ // Range is linear interpolation if values are close,
+ // and min if they are very different
+ if (fabs(ra - rb) < 0.10)
+ r = (1 - b) * ra + b * rb;
+ else r = std::min(ra, rb);
+
+ // Intensity is either-or
+ v = (int) this->myParent->GetRetro(ja) || (int) this->myParent->GetRetro(jb);
+
+ this->laserIface->data->ranges[i] = r + minRange;
+ this->laserIface->data->intensity[i] = v;
+ }
+
+ this->laserIface->Unlock();
+
+ // New data is available
+ this->laserIface->Post();
+ }
+}
+
+//////////////////////////////////////////////////////////////////////////////
+// Update the data in the interface
+void Ros_Laser::PutFiducialData()
+{
+ int i, j, count;
+ FiducialFid *fid;
+ double r, b;
+ double ax, ay, bx, by, cx, cy;
+
+ double maxAngle = this->myParent->GetMaxAngle();
+ double minAngle = this->myParent->GetMinAngle();
+
+ double maxRange = this->myParent->GetMaxRange();
+ double minRange = this->myParent->GetMinRange();
+ int rayCount = this->myParent->GetRayCount();
+ int rangeCount = this->myParent->GetRangeCount();
+
+ if (this->fiducialIface->Lock(1))
+ {
+ // Data timestamp
+ this->fiducialIface->data->head.time = Simulator::Instance()->GetSimTime();
+ this->fiducialIface->data->count = 0;
+
+ // TODO: clean this up
+ count = 0;
+ for (i = 0; i < rayCount; i++)
+ {
+ if (this->myParent->GetFiducial(i) < 0)
+ continue;
+
+ // Find the end of the fiducial
+ for (j = i + 1; j < rayCount; j++)
+ {
+ if (this->myParent->GetFiducial(j) != this->myParent->GetFiducial(i))
+ break;
+ }
+ j--;
+
+ // Need at least three points to get orientation
+ if (j - i + 1 >= 3)
+ {
+ r = minRange + this->myParent->GetRange(i);
+ b = minAngle + i * ((maxAngle-minAngle) / (rayCount - 1));
+ ax = r * cos(b);
+ ay = r * sin(b);
+
+ r = minRange + this->myParent->GetRange(j);
+ b = minAngle + j * ((maxAngle-minAngle) / (rayCount - 1));
+ bx = r * cos(b);
+ by = r * sin(b);
+
+ cx = (ax + bx) / 2;
+ cy = (ay + by) / 2;
+
+ assert(count < GZ_FIDUCIAL_MAX_FIDS);
+ fid = this->fiducialIface->data->fids + count++;
+
+ fid->id = this->myParent->GetFiducial(j);
+ fid->pose.pos.x = cx;
+ fid->pose.pos.y = cy;
+ fid->pose.yaw = atan2(by - ay, bx - ax) + M_PI / 2;
+ }
+
+ // Fewer points get no orientation
+ else
+ {
+ r = minRange + this->myParent->GetRange(i);
+ b = minAngle + i * ((maxAngle-minAngle) / (rayCount - 1));
+ ax = r * cos(b);
+ ay = r * sin(b);
+
+ r = minRange + this->myParent->GetRange(j);
+ b = minAngle + j * ((maxAngle-minAngle) / (rayCount - 1));
+ bx = r * cos(b);
+ by = r * sin(b);
+
+ cx = (ax + bx) / 2;
+ cy = (ay + by) / 2;
+
+ assert(count < GZ_FIDUCIAL_MAX_FIDS);
+ fid = this->fiducialIface->data->fids + count++;
+
+ fid->id = this->myParent->GetFiducial(j);
+ fid->pose.pos.x = cx;
+ fid->pose.pos.y = cy;
+ fid->pose.yaw = atan2(cy, cx) + M_PI;
+ }
+
+ /*printf("fiducial %d i[%d] j[%d] %.2f %.2f %.2f\n",
+ fid->id, i,j,fid->pos[0], fid->pos[1], fid->rot[2]);
+ */
+ i = j;
+ }
+
+ this->fiducialIface->data->count = count;
+
+ this->fiducialIface->Unlock();
+ this->fiducialIface->Post();
+ }
+}
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <ge...@us...> - 2008-07-25 17:05:21
|
Revision: 2130
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=2130&view=rev
Author: gerkey
Date: 2008-07-25 17:05:30 +0000 (Fri, 25 Jul 2008)
Log Message:
-----------
changed 'test' targets to 'simpletest' in preparation for standardized unit testing, which will claim the 'test' target as special
Modified Paths:
--------------
pkg/trunk/simulators/nepumuk/CMakeLists.txt
pkg/trunk/util/transforms/libTF/CMakeLists.txt
pkg/trunk/visualization/wx_ros/wx_topic_display/CMakeLists.txt
Modified: pkg/trunk/simulators/nepumuk/CMakeLists.txt
===================================================================
--- pkg/trunk/simulators/nepumuk/CMakeLists.txt 2008-07-25 08:18:51 UTC (rev 2129)
+++ pkg/trunk/simulators/nepumuk/CMakeLists.txt 2008-07-25 17:05:30 UTC (rev 2130)
@@ -14,7 +14,7 @@
include(rosbuild)
rospack(rosnpm)
rospack_add_executable(nepumuk src/simul/nepumuk.cpp)
-rospack_add_executable(test src/ros/test.cpp)
+rospack_add_executable(simpletest src/ros/test.cpp)
Modified: pkg/trunk/util/transforms/libTF/CMakeLists.txt
===================================================================
--- pkg/trunk/util/transforms/libTF/CMakeLists.txt 2008-07-25 08:18:51 UTC (rev 2129)
+++ pkg/trunk/util/transforms/libTF/CMakeLists.txt 2008-07-25 17:05:30 UTC (rev 2130)
@@ -3,8 +3,8 @@
rospack(libTF)
add_definitions(-Wall)
rospack_add_library(TF src/libTF.cpp src/Pose3DCache.cpp src/Pose3D.cpp)
-rospack_add_executable(test src/test/main.cpp)
-target_link_libraries(test TF)
+rospack_add_executable(simpletest src/test/main.cpp)
+target_link_libraries(simpletest TF)
rospack_add_executable(testtf src/test/testtf.cc)
target_link_libraries(testtf TF)
rospack_add_executable(test_interp src/test/test_interp.cc)
Modified: pkg/trunk/visualization/wx_ros/wx_topic_display/CMakeLists.txt
===================================================================
--- pkg/trunk/visualization/wx_ros/wx_topic_display/CMakeLists.txt 2008-07-25 08:18:51 UTC (rev 2129)
+++ pkg/trunk/visualization/wx_ros/wx_topic_display/CMakeLists.txt 2008-07-25 17:05:30 UTC (rev 2130)
@@ -31,8 +31,8 @@
src/wx_topic_display/GenTopicDisplay.cpp
src/wx_topic_display/TopicDisplayDialog.cpp)
-rospack_add_executable(test src/test/test.cpp
+rospack_add_executable(simpletest src/test/test.cpp
src/test/GenTestTopicDisplay.cpp)
-target_link_libraries(test wxtopicdisplay ${wxWidgets_LIBRARIES})
+target_link_libraries(simpletest wxtopicdisplay ${wxWidgets_LIBRARIES})
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <is...@us...> - 2008-07-25 20:41:31
|
Revision: 2139
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=2139&view=rev
Author: isucan
Date: 2008-07-25 20:41:38 +0000 (Fri, 25 Jul 2008)
Log Message:
-----------
made some collision space data private
Modified Paths:
--------------
pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp
pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/extension/rrt/src/RRT.cpp
pkg/trunk/util/collision_space/include/collision_space/environment.h
pkg/trunk/util/collision_space/src/collision_space/environment.cpp
pkg/trunk/util/collision_space/src/collision_space/environmentODE.cpp
pkg/trunk/util/test_collision_space/src/test_kinematic_ode.cpp
Modified: pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp 2008-07-25 20:33:21 UTC (rev 2138)
+++ pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp 2008-07-25 20:41:38 UTC (rev 2139)
@@ -254,7 +254,7 @@
unsigned int cid = m_collisionSpace->addRobotModel(*file);
model->collisionSpaceID = cid;
model->collisionSpace = m_collisionSpace;
- model->kmodel = m_collisionSpace->models[cid];
+ model->kmodel = m_collisionSpace->getModel(cid);
m_models[name] = model;
createMotionPlanningInstances(model);
@@ -265,7 +265,7 @@
std::string gname = name + "::" + groups[i];
model->collisionSpaceID = cid;
model->collisionSpace = m_collisionSpace;
- model->kmodel = m_collisionSpace->models[cid];
+ model->kmodel = m_collisionSpace->getModel(cid);
model->groupID = model->kmodel->getGroupID(gname);
m_models[gname] = model;
createMotionPlanningInstances(model);
Modified: pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/extension/rrt/src/RRT.cpp
===================================================================
--- pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/extension/rrt/src/RRT.cpp 2008-07-25 20:33:21 UTC (rev 2138)
+++ pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/extension/rrt/src/RRT.cpp 2008-07-25 20:41:38 UTC (rev 2139)
@@ -97,6 +97,9 @@
delete xstate;
delete rmotion;
-
+
+ double tsolve = solveTime - (endTime - ros::Time::now()).to_double();
+ printf("Solved in %f seconds\n", tsolve);
+
return goal_r->isAchieved();
}
Modified: pkg/trunk/util/collision_space/include/collision_space/environment.h
===================================================================
--- pkg/trunk/util/collision_space/include/collision_space/environment.h 2008-07-25 20:33:21 UTC (rev 2138)
+++ pkg/trunk/util/collision_space/include/collision_space/environment.h 2008-07-25 20:41:38 UTC (rev 2139)
@@ -57,8 +57,8 @@
virtual ~EnvironmentModel(void)
{
- for (unsigned int i = 0 ; i < models.size() ; ++i)
- delete models[i];
+ for (unsigned int i = 0 ; i < m_models.size() ; ++i)
+ delete m_models[i];
}
/** Check if a model is in collision */
@@ -78,6 +78,11 @@
/** Update the positions of the geometry used in collision detection */
virtual void updateRobotModel(unsigned int model_id) = 0;
+ /** Get the number of loaded models */
+ unsigned int getModelCount(void) const;
+ /** Get a specific model */
+ planning_models::KinematicModel* getModel(unsigned int model_id) const;
+
/** Provide interface to a lock. Use carefully! */
void lock(void);
@@ -89,15 +94,15 @@
/** Check if self collision is enabled */
bool getSelfCollision(void) const;
-
- /** List of loaded robot models */
- std::vector<planning_models::KinematicModel*> models;
protected:
ros::thread::mutex m_lock;
bool m_selfCollision;
+ /** List of loaded robot models */
+ std::vector<planning_models::KinematicModel*> m_models;
+
};
}
Modified: pkg/trunk/util/collision_space/src/collision_space/environment.cpp
===================================================================
--- pkg/trunk/util/collision_space/src/collision_space/environment.cpp 2008-07-25 20:33:21 UTC (rev 2138)
+++ pkg/trunk/util/collision_space/src/collision_space/environment.cpp 2008-07-25 20:41:38 UTC (rev 2139)
@@ -43,8 +43,8 @@
unsigned int collision_space::EnvironmentModel::addRobotModel(planning_models::KinematicModel *model)
{
- unsigned int pos = models.size();
- models.push_back(model);
+ unsigned int pos = m_models.size();
+ m_models.push_back(model);
return pos;
}
@@ -67,3 +67,14 @@
{
return m_selfCollision;
}
+
+
+unsigned int collision_space::EnvironmentModel::getModelCount(void) const
+{
+ return m_models.size();
+}
+
+planning_models::KinematicModel* collision_space::EnvironmentModel::getModel(unsigned int model_id) const
+{
+ return m_models[model_id];
+}
Modified: pkg/trunk/util/collision_space/src/collision_space/environmentODE.cpp
===================================================================
--- pkg/trunk/util/collision_space/src/collision_space/environmentODE.cpp 2008-07-25 20:33:21 UTC (rev 2138)
+++ pkg/trunk/util/collision_space/src/collision_space/environmentODE.cpp 2008-07-25 20:41:38 UTC (rev 2139)
@@ -56,9 +56,9 @@
m_kgeoms[id].s = dHashSpaceCreate(0);
}
- for (unsigned int j = 0 ; j < models[id]->getRobotCount() ; ++j)
+ for (unsigned int j = 0 ; j < m_models[id]->getRobotCount() ; ++j)
{
- planning_models::KinematicModel::Robot *robot = models[id]->getRobot(j);
+ planning_models::KinematicModel::Robot *robot = m_models[id]->getRobot(j);
for (unsigned int i = 0 ; i < robot->links.size() ; ++i)
{
kGeom *kg = new kGeom();
Modified: pkg/trunk/util/test_collision_space/src/test_kinematic_ode.cpp
===================================================================
--- pkg/trunk/util/test_collision_space/src/test_kinematic_ode.cpp 2008-07-25 20:33:21 UTC (rev 2138)
+++ pkg/trunk/util/test_collision_space/src/test_kinematic_ode.cpp 2008-07-25 20:41:38 UTC (rev 2139)
@@ -95,7 +95,7 @@
EnvironmentModel *km = new EnvironmentModelODE();
km->addRobotModel(model);
- planning_models::KinematicModel *m = km->models[0];
+ planning_models::KinematicModel *m = km->getModel(0);
printModelInfo(m);
double *param = new double[m->stateDimension];
@@ -110,9 +110,13 @@
param[i] = 0.1;
m->computeTransforms(param, m->getGroupID("pr2::leftArm"));
km->updateRobotModel(0);
-
- spaces.addSpace(dynamic_cast<EnvironmentModelODE*>(km)->getModelODESpace(0));
+ EnvironmentModelODE* okm = dynamic_cast<EnvironmentModelODE*>(km);
+ spaces.addSpace(okm->getODESpace());
+ for (unsigned int i = 0 ; i < okm->getModelCount() ; ++i)
+ spaces.addSpace(okm->getModelODESpace(i));
+
+
dsFunctions fn;
fn.version = DS_VERSION;
fn.start = &start;
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <is...@us...> - 2008-07-25 21:11:40
|
Revision: 2141
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=2141&view=rev
Author: isucan
Date: 2008-07-25 21:11:48 +0000 (Fri, 25 Jul 2008)
Log Message:
-----------
separated display utility as well
Modified Paths:
--------------
pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp
pkg/trunk/util/collision_space/src/collision_space/environmentODE.cpp
pkg/trunk/util/test_collision_space/manifest.xml
pkg/trunk/util/test_collision_space/src/test_kinematic_ode.cpp
Added Paths:
-----------
pkg/trunk/util/display_ode_spaces/
pkg/trunk/util/display_ode_spaces/CMakeLists.txt
pkg/trunk/util/display_ode_spaces/Makefile
pkg/trunk/util/display_ode_spaces/include/
pkg/trunk/util/display_ode_spaces/include/display_ode/
pkg/trunk/util/display_ode_spaces/include/display_ode/displayODE.h
pkg/trunk/util/display_ode_spaces/manifest.xml
pkg/trunk/util/display_ode_spaces/src/
pkg/trunk/util/display_ode_spaces/src/displayODE.cpp
Removed Paths:
-------------
pkg/trunk/util/test_collision_space/src/displayODE.h
Modified: pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp 2008-07-25 20:56:01 UTC (rev 2140)
+++ pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp 2008-07-25 21:11:48 UTC (rev 2141)
@@ -104,7 +104,8 @@
subscribe("world_3d_map", m_cloud, &KinematicPlanning::pointCloudCallback);
m_collisionSpace = new collision_space::EnvironmentModelODE();
-
+ m_collisionSpace->setSelfCollision(false); // for now, disable self collision (incorrect model)
+
m_collisionSpace->lock();
loadRobotDescriptions();
m_collisionSpace->unlock();
Modified: pkg/trunk/util/collision_space/src/collision_space/environmentODE.cpp
===================================================================
--- pkg/trunk/util/collision_space/src/collision_space/environmentODE.cpp 2008-07-25 20:56:01 UTC (rev 2140)
+++ pkg/trunk/util/collision_space/src/collision_space/environmentODE.cpp 2008-07-25 21:11:48 UTC (rev 2141)
@@ -178,7 +178,6 @@
return m_kgeoms[model_id].s;
}
-
struct CollisionData
{
bool collides;
Added: pkg/trunk/util/display_ode_spaces/CMakeLists.txt
===================================================================
--- pkg/trunk/util/display_ode_spaces/CMakeLists.txt (rev 0)
+++ pkg/trunk/util/display_ode_spaces/CMakeLists.txt 2008-07-25 21:11:48 UTC (rev 2141)
@@ -0,0 +1,5 @@
+cmake_minimum_required(VERSION 2.6)
+include(rosbuild)
+rospack(display_ode_spaces)
+
+rospack_add_library(display_ODE_space src/displayODE.cpp)
Added: pkg/trunk/util/display_ode_spaces/Makefile
===================================================================
--- pkg/trunk/util/display_ode_spaces/Makefile (rev 0)
+++ pkg/trunk/util/display_ode_spaces/Makefile 2008-07-25 21:11:48 UTC (rev 2141)
@@ -0,0 +1 @@
+include $(shell rospack find mk)/cmake.mk
Copied: pkg/trunk/util/display_ode_spaces/include/display_ode/displayODE.h (from rev 2137, pkg/trunk/util/test_collision_space/src/displayODE.h)
===================================================================
--- pkg/trunk/util/display_ode_spaces/include/display_ode/displayODE.h (rev 0)
+++ pkg/trunk/util/display_ode_spaces/include/display_ode/displayODE.h 2008-07-25 21:11:48 UTC (rev 2141)
@@ -0,0 +1,123 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2008, Willow Garage, Inc.
+* All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* * Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* * Redistributions in binary form must reproduce the above
+* copyright notice, this list of conditions and the following
+* disclaimer in the documentation and/or other materials provided
+* with the distribution.
+* * Neither the name of the Willow Garage nor the names of its
+* contributors may be used to endorse or promote products derived
+* from this software without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+#include <ode/ode.h>
+#include <drawstuff/drawstuff.h>
+#include <vector>
+
+#ifdef dDOUBLE
+#define dsDrawBox dsDrawBoxD
+#define dsDrawSphere dsDrawSphereD
+#define dsDrawTriangle dsDrawTriangleD
+#define dsDrawCylinder dsDrawCylinderD
+#define dsDrawCapsule dsDrawCapsuleD
+#define dsDrawLine dsDrawLineD
+#define dsDrawConvex dsDrawConvexD
+#endif
+
+namespace display_ode
+{
+
+ class DisplayODESpaces
+ {
+ public:
+
+ void drawSphere(dGeomID geom)
+ {
+ dReal radius = dGeomSphereGetRadius(geom);
+ dsDrawSphere(dGeomGetPosition(geom), dGeomGetRotation(geom), radius);
+ }
+
+ void drawBox(dGeomID geom)
+ {
+ dVector3 sides;
+ dGeomBoxGetLengths(geom, sides);
+ dsDrawBox(dGeomGetPosition(geom), dGeomGetRotation(geom), sides);
+ }
+
+ void drawCylinder(dGeomID geom)
+ {
+ dReal radius, length;
+ dGeomCylinderGetParams(geom, &radius, &length);
+ dsDrawCylinder(dGeomGetPosition(geom), dGeomGetRotation(geom), length, radius);
+ }
+
+ void displaySpace(dSpaceID space)
+ {
+ int ngeoms = dSpaceGetNumGeoms(space);
+ for (int i = 0 ; i < ngeoms ; ++i)
+ {
+ dGeomID geom = dSpaceGetGeom(space, i);
+ int cls = dGeomGetClass(geom);
+ switch (cls)
+ {
+ case dSphereClass:
+ drawSphere(geom);
+ break;
+ case dBoxClass:
+ drawBox(geom);
+ break;
+ case dCylinderClass:
+ drawCylinder(geom);
+ break;
+ default:
+ printf("Geometry class %d not yet implemented\n", cls);
+ break;
+ }
+ }
+ }
+
+ void displaySpaces(void)
+ {
+ for (unsigned int i = 0 ; i < m_spaces.size() ; ++i)
+ displaySpace(m_spaces[i]);
+ }
+
+ void addSpace(dSpaceID space)
+ {
+ m_spaces.push_back(space);
+ }
+
+ void clear(void)
+ {
+ m_spaces.clear();
+ }
+
+ protected:
+
+ std::vector<dSpaceID> m_spaces;
+
+ };
+
+}
Added: pkg/trunk/util/display_ode_spaces/manifest.xml
===================================================================
--- pkg/trunk/util/display_ode_spaces/manifest.xml (rev 0)
+++ pkg/trunk/util/display_ode_spaces/manifest.xml 2008-07-25 21:11:48 UTC (rev 2141)
@@ -0,0 +1,15 @@
+<package>
+ <description brief="Simple library for displaying ODE spaces">
+ Given a set of ODE spaces, display them using the drawstuff library
+ </description>
+
+ <author>Ioan Sucan/is...@wi...</author>
+ <license>BSD</license>
+
+ <depend package="drawstuff"/>
+
+ <export>
+ <cpp cflags="-I${prefix}/include" lflags="-Wl,-rpath,${prefix}/lib -L${prefix}/lib -ldisplay_ODE_space"/>
+ </export>
+
+</package>
Added: pkg/trunk/util/display_ode_spaces/src/displayODE.cpp
===================================================================
--- pkg/trunk/util/display_ode_spaces/src/displayODE.cpp (rev 0)
+++ pkg/trunk/util/display_ode_spaces/src/displayODE.cpp 2008-07-25 21:11:48 UTC (rev 2141)
@@ -0,0 +1,35 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2008, Willow Garage, Inc.
+* All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* * Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* * Redistributions in binary form must reproduce the above
+* copyright notice, this list of conditions and the following
+* disclaimer in the documentation and/or other materials provided
+* with the distribution.
+* * Neither the name of the Willow Garage nor the names of its
+* contributors may be used to endorse or promote products derived
+* from this software without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+#include <display_ode/displayODE.h>
Modified: pkg/trunk/util/test_collision_space/manifest.xml
===================================================================
--- pkg/trunk/util/test_collision_space/manifest.xml 2008-07-25 20:56:01 UTC (rev 2140)
+++ pkg/trunk/util/test_collision_space/manifest.xml 2008-07-25 21:11:48 UTC (rev 2141)
@@ -7,6 +7,6 @@
<license>BSD</license>
<depend package="collision_space"/>
- <depend package="drawstuff"/>
+ <depend package="display_ode_spaces"/>
</package>
Deleted: pkg/trunk/util/test_collision_space/src/displayODE.h
===================================================================
--- pkg/trunk/util/test_collision_space/src/displayODE.h 2008-07-25 20:56:01 UTC (rev 2140)
+++ pkg/trunk/util/test_collision_space/src/displayODE.h 2008-07-25 21:11:48 UTC (rev 2141)
@@ -1,119 +0,0 @@
-/*********************************************************************
-* Software License Agreement (BSD License)
-*
-* Copyright (c) 2008, Willow Garage, Inc.
-* All rights reserved.
-*
-* Redistribution and use in source and binary forms, with or without
-* modification, are permitted provided that the following conditions
-* are met:
-*
-* * Redistributions of source code must retain the above copyright
-* notice, this list of conditions and the following disclaimer.
-* * Redistributions in binary form must reproduce the above
-* copyright notice, this list of conditions and the following
-* disclaimer in the documentation and/or other materials provided
-* with the distribution.
-* * Neither the name of the Willow Garage nor the names of its
-* contributors may be used to endorse or promote products derived
-* from this software without specific prior written permission.
-*
-* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
-* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
-* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-* POSSIBILITY OF SUCH DAMAGE.
-*********************************************************************/
-
-#include <ode/ode.h>
-#include <drawstuff/drawstuff.h>
-#include <vector>
-
-#ifdef dDOUBLE
-#define dsDrawBox dsDrawBoxD
-#define dsDrawSphere dsDrawSphereD
-#define dsDrawTriangle dsDrawTriangleD
-#define dsDrawCylinder dsDrawCylinderD
-#define dsDrawCapsule dsDrawCapsuleD
-#define dsDrawLine dsDrawLineD
-#define dsDrawConvex dsDrawConvexD
-#endif
-
-class DisplayODESpaces
-{
- public:
-
- void drawSphere(dGeomID geom)
- {
- dReal radius = dGeomSphereGetRadius(geom);
- dsDrawSphere(dGeomGetPosition(geom), dGeomGetRotation(geom), radius);
- }
-
- void drawBox(dGeomID geom)
- {
- dVector3 sides;
- dGeomBoxGetLengths(geom, sides);
- dsDrawBox(dGeomGetPosition(geom), dGeomGetRotation(geom), sides);
- }
-
- void drawCylinder(dGeomID geom)
- {
- dReal radius, length;
- dGeomCylinderGetParams(geom, &radius, &length);
- dsDrawCylinder(dGeomGetPosition(geom), dGeomGetRotation(geom), length, radius);
- }
-
- void displaySpace(dSpaceID space)
- {
- int ngeoms = dSpaceGetNumGeoms(space);
- for (int i = 0 ; i < ngeoms ; ++i)
- {
- dGeomID geom = dSpaceGetGeom(space, i);
- int cls = dGeomGetClass(geom);
- switch (cls)
- {
- case dSphereClass:
- drawSphere(geom);
- break;
- case dBoxClass:
- drawBox(geom);
- break;
- case dCylinderClass:
- drawCylinder(geom);
- break;
- default:
- printf("Geometry class %d not yet implemented\n", cls);
- break;
- }
- }
- }
-
- void displaySpaces(void)
- {
- for (unsigned int i = 0 ; i < m_spaces.size() ; ++i)
- displaySpace(m_spaces[i]);
- }
-
- void addSpace(dSpaceID space)
- {
- m_spaces.push_back(space);
- }
-
- void clear(void)
- {
- m_spaces.clear();
- }
-
- protected:
-
- std::vector<dSpaceID> m_spaces;
-
-};
-
Modified: pkg/trunk/util/test_collision_space/src/test_kinematic_ode.cpp
===================================================================
--- pkg/trunk/util/test_collision_space/src/test_kinematic_ode.cpp 2008-07-25 20:56:01 UTC (rev 2140)
+++ pkg/trunk/util/test_collision_space/src/test_kinematic_ode.cpp 2008-07-25 21:11:48 UTC (rev 2141)
@@ -33,8 +33,9 @@
*********************************************************************/
#include <collision_space/environmentODE.h>
-#include "displayODE.h"
+#include <display_ode/displayODE.h>
using namespace collision_space;
+using namespace display_ode;
static DisplayODESpaces spaces;
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <is...@us...> - 2008-07-26 00:05:40
|
Revision: 2155
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=2155&view=rev
Author: isucan
Date: 2008-07-26 00:05:50 +0000 (Sat, 26 Jul 2008)
Log Message:
-----------
fixed clear function in collision space
Modified Paths:
--------------
pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp
pkg/trunk/util/collision_space/src/collision_space/environmentODE.cpp
Modified: pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp 2008-07-26 00:04:01 UTC (rev 2154)
+++ pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp 2008-07-26 00:05:50 UTC (rev 2155)
@@ -119,7 +119,7 @@
m_collisionSpace->unlock();
// temp obstacle
- double sphere[3] = {0.8,0.2,0.4};
+ double sphere[6] = {0.8,0.2,0.4};
m_collisionSpace->addPointCloud(1, sphere, 0.15);
#ifdef DISPLAY_ODE_SPACES
@@ -162,25 +162,42 @@
void pointCloudCallback(void)
{
- unsigned int n = m_cloud.get_pts_size();
+ const int frac = 50;
+ unsigned int n = m_cloud.get_pts_size()/frac;
printf("received %u points\n", n);
+
+#ifdef DISPLAY_ODE_SPACES
+ spaces.clear();
+#endif
+
+
ros::Time startTime = ros::Time::now();
double *data = new double[3 * n];
for (unsigned int i = 0 ; i < n ; ++i)
{
unsigned int i3 = i * 3;
- data[i3 ] = m_cloud.pts[i].x;
- data[i3 + 1] = m_cloud.pts[i].y;
- data[i3 + 2] = m_cloud.pts[i].z;
+ data[i3 ] = m_cloud.pts[i * frac].x;
+ data[i3 + 1] = m_cloud.pts[i * frac].y;
+ data[i3 + 2] = m_cloud.pts[i * frac].z;
}
m_collisionSpace->lock();
m_collisionSpace->clearObstacles();
- m_collisionSpace->addPointCloud(n, data, 0.01);
+ m_collisionSpace->addPointCloud(n, data, 0.03);
m_collisionSpace->unlock();
delete[] data;
+
+#ifdef DISPLAY_ODE_SPACES
+ collision_space::EnvironmentModelODE* okm = dynamic_cast<collision_space::EnvironmentModelODE*>(m_collisionSpace);
+ if (okm)
+ {
+ spaces.addSpace(okm->getODESpace(), 1.0f, 0.0f, 0.0f);
+ for (unsigned int i = 0 ; i < okm->getModelCount() ; ++i)
+ spaces.addSpace(okm->getModelODESpace(i), 0.1f, 0.5f, (float)(i + 1)/(float)okm->getModelCount());
+ }
+#endif
double tupd = (ros::Time::now() - startTime).to_double();
printf("Updated world model in %f seconds\n", tupd);
Modified: pkg/trunk/util/collision_space/src/collision_space/environmentODE.cpp
===================================================================
--- pkg/trunk/util/collision_space/src/collision_space/environmentODE.cpp 2008-07-26 00:04:01 UTC (rev 2154)
+++ pkg/trunk/util/collision_space/src/collision_space/environmentODE.cpp 2008-07-26 00:05:50 UTC (rev 2155)
@@ -226,7 +226,8 @@
void collision_space::EnvironmentModelODE::clearObstacles(void)
{
m_collide2.clear();
- freeMemory();
+ if (m_space)
+ dSpaceDestroy(m_space);
m_space = dHashSpaceCreate(0);
m_collide2.setup();
}
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <stu...@us...> - 2008-07-28 22:39:03
|
Revision: 2197
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=2197&view=rev
Author: stuglaser
Date: 2008-07-28 22:39:11 +0000 (Mon, 28 Jul 2008)
Log Message:
-----------
Fixed dependencies on transmission and robot model.
Modified Paths:
--------------
pkg/trunk/mechanism/mechanism_control/include/mechanism_control/mechanism_control.h
pkg/trunk/mechanism/mechanism_control/src/base_control.cpp
pkg/trunk/mechanism/mechanism_control/src/gazebo_control.cpp
pkg/trunk/robot_models/robot_mechanism_model/include/mechanism_model/robot.h
pkg/trunk/robot_models/robot_mechanism_model/include/mechanism_model/transmission.h
Modified: pkg/trunk/mechanism/mechanism_control/include/mechanism_control/mechanism_control.h
===================================================================
--- pkg/trunk/mechanism/mechanism_control/include/mechanism_control/mechanism_control.h 2008-07-28 22:18:58 UTC (rev 2196)
+++ pkg/trunk/mechanism/mechanism_control/include/mechanism_control/mechanism_control.h 2008-07-28 22:39:11 UTC (rev 2197)
@@ -31,6 +31,7 @@
#include <map>
#include <string>
#include <vector>
+#include "tinyxml-2.5.3/tinyxml.h"
#include <hw_interface/hardware_interface.h>
#include <mechanism_model/robot.h>
#include <rosthread/mutex.h>
Modified: pkg/trunk/mechanism/mechanism_control/src/base_control.cpp
===================================================================
--- pkg/trunk/mechanism/mechanism_control/src/base_control.cpp 2008-07-28 22:18:58 UTC (rev 2196)
+++ pkg/trunk/mechanism/mechanism_control/src/base_control.cpp 2008-07-28 22:39:11 UTC (rev 2197)
@@ -2,27 +2,27 @@
///////////////////////////////////////////////////////////////////////////////
// Copyright (C) 2008, Willow Garage Inc.
//
-// Redistribution and use in source and binary forms, with or without
+// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
-// * Redistributions of source code must retain the above copyright notice,
+// * Redistributions of source code must retain the above copyright notice,
// this list of conditions and the following disclaimer.
-// * Redistributions in binary form must reproduce the above copyright
-// notice, this list of conditions and the following disclaimer in the
+// * Redistributions in binary form must reproduce the above copyright
+// notice, this list of conditions and the following disclaimer in the
// documentation and/or other materials provided with the distribution.
-// * Neither the name of Willow Garage Inc. nor the names of its
-// contributors may be used to endorse or promote products derived from
+// * Neither the name of Willow Garage Inc. nor the names of its
+// contributors may be used to endorse or promote products derived from
// this software without specific prior written permission.
-//
-// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
-// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
-// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
-// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
-// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
-// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
-// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
-// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
-// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
-// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
//////////////////////////////////////////////////////////////////////////////
@@ -33,7 +33,7 @@
#define NUM_JOINTS 12
-const double maxPositiveTorque = 0.75;
+const double maxPositiveTorque = 0.75;
using namespace std;
@@ -51,22 +51,22 @@
}
void BaseControl::initRobot(){
- r = new Robot("robot");
+ r = new Robot("robot");
r->numJoints = NUM_JOINTS;
- r->numTransmissions = NUM_JOINTS;
r->numLinks = NUM_JOINTS;
- r->transmission = new SimpleTransmission[NUM_JOINTS];
r->joint = new Joint[NUM_JOINTS];
r->link = new Link[NUM_JOINTS];
for(int ii=0; ii<NUM_JOINTS; ii++){
- r->transmission[ii].actuator = &(hw->actuator[ii]);
- r->transmission[ii].joint = &(r->joint[ii]);
- r->transmission[ii].mechanicalReduction = 1.0;
- r->transmission[ii].motorTorqueConstant = 1.0;
- r->transmission[ii].pulsesPerRevolution = 90000;
+ SimpleTransmission *tr = new SimpleTransmission;
+ r->transmissions_.push_back(tr);
+ tr->actuator = &(hw->actuator[ii]);
+ tr->joint = &(r->joint[ii]);
+ tr->mechanicalReduction = 1.0;
+ tr->motorTorqueConstant = 1.0;
+ tr->pulsesPerRevolution = 90000;
hw->actuator[ii].command.enable = true;
r->joint[ii].effortLimit = maxPositiveTorque;
}
@@ -86,8 +86,8 @@
//Clear actuator commands
//Process robot model transmissions
- for(int i = 0; i < r->numTransmissions; i++){
- r->transmission[i].propagatePosition();
+ for(int i = 0; i < r->transmissions_.size(); i++){
+ r->transmissions_[i]->propagatePosition();
}
//update KDL model with new joint position/velocities
@@ -101,8 +101,8 @@
r->joint[i].enforceLimits();
}*/
- for(int i = 0; i < r->numTransmissions; i++){
- r->transmission[i].propagateEffort();
+ for(int i = 0; i < r->transmissions_.size(); i++){
+ r->transmissions_[i]->propagateEffort();
}
}
Modified: pkg/trunk/mechanism/mechanism_control/src/gazebo_control.cpp
===================================================================
--- pkg/trunk/mechanism/mechanism_control/src/gazebo_control.cpp 2008-07-28 22:18:58 UTC (rev 2196)
+++ pkg/trunk/mechanism/mechanism_control/src/gazebo_control.cpp 2008-07-28 22:39:11 UTC (rev 2197)
@@ -2,27 +2,27 @@
///////////////////////////////////////////////////////////////////////////////
// Copyright (C) 2008, Willow Garage Inc.
//
-// Redistribution and use in source and binary forms, with or without
+// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
-// * Redistributions of source code must retain the above copyright notice,
+// * Redistributions of source code must retain the above copyright notice,
// this list of conditions and the following disclaimer.
-// * Redistributions in binary form must reproduce the above copyright
-// notice, this list of conditions and the following disclaimer in the
+// * Redistributions in binary form must reproduce the above copyright
+// notice, this list of conditions and the following disclaimer in the
// documentation and/or other materials provided with the distribution.
-// * Neither the name of Willow Garage Inc. nor the names of its
-// contributors may be used to endorse or promote products derived from
+// * Neither the name of Willow Garage Inc. nor the names of its
+// contributors may be used to endorse or promote products derived from
// this software without specific prior written permission.
-//
-// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
-// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
-// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
-// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
-// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
-// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
-// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
-// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
-// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
-// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
//////////////////////////////////////////////////////////////////////////////
@@ -55,24 +55,24 @@
}
void GazeboMechanismControl::initRobot(){
- r = new Robot("robot");
+ r = new Robot((char*)"robot");
r->numJoints = PR2::MAX_JOINTS;
- r->numTransmissions = PR2::MAX_JOINTS;
r->numLinks = PR2::MAX_JOINTS;
- r->transmission = new SimpleTransmission[PR2::MAX_JOINTS];
r->joint = new Joint[PR2::MAX_JOINTS];
r->link = new Link[PR2::MAX_JOINTS];
// MAPPING BETWEEN hwInterface->actuators and robot->joints
// assign transmissions for all joints
for(int ii=0; ii<PR2::MAX_JOINTS; ii++){
- r->transmission[ii].actuator = &(hardwareInterface->actuator[ii]);
- r->transmission[ii].joint = &(r->joint[ii]);
- r->transmission[ii].mechanicalReduction = 1.0;
- r->transmission[ii].motorTorqueConstant = 1.0;
- r->transmission[ii].pulsesPerRevolution = 1.0;
+ SimpleTransmission *tr = new SimpleTransmission;
+ r->transmissions_.push_back(tr);
+ tr->actuator = &(hardwareInterface->actuator[ii]);
+ tr->joint = &(r->joint[ii]);
+ tr->mechanicalReduction = 1.0;
+ tr->motorTorqueConstant = 1.0;
+ tr->pulsesPerRevolution = 1.0;
r->joint[ii].effortLimit = maxPositiveTorque;
}
// enable all actuators, just so happens num. joints == num. actuators for now
@@ -85,44 +85,44 @@
// assuming that: robot joint array == hardware interface actuator array
// hard coded mapping between jointcontroller array and robot joint array
- int baseMapToRobotJointIndex[] = {
+ int baseMapToRobotJointIndex[] = {
PR2::CASTER_FL_STEER , PR2::CASTER_FL_DRIVE_L , PR2::CASTER_FL_DRIVE_R ,
PR2::CASTER_FR_STEER , PR2::CASTER_FR_DRIVE_L , PR2::CASTER_FR_DRIVE_R ,
PR2::CASTER_RL_STEER , PR2::CASTER_RL_DRIVE_L , PR2::CASTER_RL_DRIVE_R ,
PR2::CASTER_RR_STEER , PR2::CASTER_RR_DRIVE_L , PR2::CASTER_RR_DRIVE_R };
- int leftArmMapToRobotJointIndex[] = {
+ int leftArmMapToRobotJointIndex[] = {
PR2::ARM_L_PAN , PR2::ARM_L_SHOULDER_PITCH, PR2::ARM_L_SHOULDER_ROLL ,
PR2::ARM_L_ELBOW_PITCH , PR2::ARM_L_ELBOW_ROLL , PR2::ARM_L_WRIST_PITCH ,
PR2::ARM_L_WRIST_ROLL };
- int rightArmMapToRobotJointIndex[] = {
+ int rightArmMapToRobotJointIndex[] = {
PR2::ARM_R_PAN , PR2::ARM_R_SHOULDER_PITCH, PR2::ARM_R_SHOULDER_ROLL ,
PR2::ARM_R_ELBOW_PITCH , PR2::ARM_R_ELBOW_ROLL , PR2::ARM_R_WRIST_PITCH ,
PR2::ARM_R_WRIST_ROLL };
- int headMapToRobotJointIndex[] = {
+ int headMapToRobotJointIndex[] = {
PR2::HEAD_YAW , PR2::HEAD_PITCH };
- int laserScannerMapToRobotJointIndex[] = {
+ int laserScannerMapToRobotJointIndex[] = {
PR2::HEAD_LASER_PITCH };
- int spineMapToRobotJointIndex[] = {
+ int spineMapToRobotJointIndex[] = {
PR2::SPINE_ELEVATOR };
// hard coded mapping between jointcontroller array and hardware interface actuator array
- int baseMapToHIActuatorIndex[] = {
+ int baseMapToHIActuatorIndex[] = {
PR2::CASTER_FL_STEER , PR2::CASTER_FL_DRIVE_L , PR2::CASTER_FL_DRIVE_R ,
PR2::CASTER_FR_STEER , PR2::CASTER_FR_DRIVE_L , PR2::CASTER_FR_DRIVE_R ,
PR2::CASTER_RL_STEER , PR2::CASTER_RL_DRIVE_L , PR2::CASTER_RL_DRIVE_R ,
PR2::CASTER_RR_STEER , PR2::CASTER_RR_DRIVE_L , PR2::CASTER_RR_DRIVE_R };
- int leftArmMapToHIActuatorIndex[] = {
+ int leftArmMapToHIActuatorIndex[] = {
PR2::ARM_L_PAN , PR2::ARM_L_SHOULDER_PITCH, PR2::ARM_L_SHOULDER_ROLL ,
PR2::ARM_L_ELBOW_PITCH , PR2::ARM_L_ELBOW_ROLL , PR2::ARM_L_WRIST_PITCH ,
PR2::ARM_L_WRIST_ROLL };
- int rightArmMapToHIActuatorIndex[] = {
+ int rightArmMapToHIActuatorIndex[] = {
PR2::ARM_R_PAN , PR2::ARM_R_SHOULDER_PITCH, PR2::ARM_R_SHOULDER_ROLL ,
PR2::ARM_R_ELBOW_PITCH , PR2::ARM_R_ELBOW_ROLL , PR2::ARM_R_WRIST_PITCH ,
PR2::ARM_R_WRIST_ROLL };
- int headMapToHIActuatorIndex[] = {
+ int headMapToHIActuatorIndex[] = {
PR2::HEAD_YAW , PR2::HEAD_PITCH };
- int laserScannerMapToHIActuatorIndex[] = {
+ int laserScannerMapToHIActuatorIndex[] = {
PR2::HEAD_LASER_PITCH };
- int spineMapToHIActuatorIndex[] = {
+ int spineMapToHIActuatorIndex[] = {
PR2::SPINE_ELEVATOR };
int leftArmNumJoints = 7;
@@ -165,8 +165,8 @@
//Clear actuator commands
//Process robot model transmissions
- for(int i = 0; i < r->numTransmissions; i++){
- r->transmission[i].propagatePosition();
+ for(int i = 0; i < r->transmissions_.size(); i++){
+ r->transmissions_[i]->propagatePosition();
}
//update KDL model with new joint position/velocities
@@ -183,8 +183,8 @@
r->joint[i].enforceLimits();
}*/
- for(int i = 0; i < r->numTransmissions; i++){
- r->transmission[i].propagateEffort();
+ for(int i = 0; i < r->transmissions_.size(); i++){
+ r->transmissions_[i]->propagateEffort();
}
}
Modified: pkg/trunk/robot_models/robot_mechanism_model/include/mechanism_model/robot.h
===================================================================
--- pkg/trunk/robot_models/robot_mechanism_model/include/mechanism_model/robot.h 2008-07-28 22:18:58 UTC (rev 2196)
+++ pkg/trunk/robot_models/robot_mechanism_model/include/mechanism_model/robot.h 2008-07-28 22:39:11 UTC (rev 2197)
@@ -52,7 +52,7 @@
Joint *joint;
int numJoints;
- vector<Transmission*> transmissions_;
+ std::vector<Transmission*> transmissions_;
};
}
Modified: pkg/trunk/robot_models/robot_mechanism_model/include/mechanism_model/transmission.h
===================================================================
--- pkg/trunk/robot_models/robot_mechanism_model/include/mechanism_model/transmission.h 2008-07-28 22:18:58 UTC (rev 2196)
+++ pkg/trunk/robot_models/robot_mechanism_model/include/mechanism_model/transmission.h 2008-07-28 22:39:11 UTC (rev 2197)
@@ -48,7 +48,7 @@
};
-class SimpleTransmission : Transmission
+class SimpleTransmission : public Transmission
{
public:
SimpleTransmission() {}
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <hsu...@us...> - 2008-07-28 22:57:41
|
Revision: 2203
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=2203&view=rev
Author: hsujohnhsu
Date: 2008-07-28 22:57:50 +0000 (Mon, 28 Jul 2008)
Log Message:
-----------
added times and frameid's for all cameras and lasers.
Modified Paths:
--------------
pkg/trunk/drivers/robot/pr2/pr2Core/include/pr2Core/pr2Core.h
pkg/trunk/simulators/rosgazebo/include/RosGazeboNode/RosGazeboNode.h
pkg/trunk/simulators/rosgazebo/src/RosGazeboNode.cpp
Modified: pkg/trunk/drivers/robot/pr2/pr2Core/include/pr2Core/pr2Core.h
===================================================================
--- pkg/trunk/drivers/robot/pr2/pr2Core/include/pr2Core/pr2Core.h 2008-07-28 22:46:22 UTC (rev 2202)
+++ pkg/trunk/drivers/robot/pr2/pr2Core/include/pr2Core/pr2Core.h 2008-07-28 22:57:50 UTC (rev 2203)
@@ -45,6 +45,12 @@
"FRAMEID_STEREO_BLOCK" ,//34
"FRAMEID_TILT_LASER_BLOCK" ,//35
"FRAMEID_BASE_LASER_BLOCK" ,//36
+ "FRAMEID_PTZ_CAM_L" ,//37
+ "FRAMEID_PTZ_CAM_R" ,//38
+ "FRAMEID_FOREARM_CAM_L" ,//39
+ "FRAMEID_FOREARM_CAM_R" ,//40
+ "FRAMEID_WRIST_CAM_L" ,//41
+ "FRAMEID_WRIST_CAM_R" ,//42
"END"
};
Modified: pkg/trunk/simulators/rosgazebo/include/RosGazeboNode/RosGazeboNode.h
===================================================================
--- pkg/trunk/simulators/rosgazebo/include/RosGazeboNode/RosGazeboNode.h 2008-07-28 22:46:22 UTC (rev 2202)
+++ pkg/trunk/simulators/rosgazebo/include/RosGazeboNode/RosGazeboNode.h 2008-07-28 22:57:50 UTC (rev 2203)
@@ -156,6 +156,7 @@
// laser range data
float ranges[GZ_LASER_MAX_RANGES];
uint8_t intensities[GZ_LASER_MAX_RANGES];
+ double cameraTime, laserTime;
// camera data
std_msgs::Image img;
Modified: pkg/trunk/simulators/rosgazebo/src/RosGazeboNode.cpp
===================================================================
--- pkg/trunk/simulators/rosgazebo/src/RosGazeboNode.cpp 2008-07-28 22:46:22 UTC (rev 2202)
+++ pkg/trunk/simulators/rosgazebo/src/RosGazeboNode.cpp 2008-07-28 22:57:50 UTC (rev 2203)
@@ -444,7 +444,7 @@
&angle_min, &angle_max, &angle_increment,
&range_max, &ranges_size , &ranges_alloc_size,
&intensities_size, &intensities_alloc_size,
- this->ranges , this->intensities) == PR2::PR2_ALL_OK)
+ this->ranges , this->intensities, &laserTime) == PR2::PR2_ALL_OK)
{
for(unsigned int i=0;i<ranges_size;i++)
{
@@ -500,8 +500,8 @@
//std::cout << " pcd num " << this->cloud_pts->length << std::endl;
//
this->cloudMsg.header.frame_id = tf.lookup("FRAMEID_BASE");
- this->cloudMsg.header.stamp.sec = (unsigned long)floor(this->simTime);
- this->cloudMsg.header.stamp.nsec = (unsigned long)floor( 1e9 * ( this->simTime - this->cloudMsg.header.stamp.sec) );
+ this->cloudMsg.header.stamp.sec = (unsigned long)floor(this->laserTime);
+ this->cloudMsg.header.stamp.nsec = (unsigned long)floor( 1e9 * ( this->laserTime - this->cloudMsg.header.stamp.sec) );
int num_channels = 1;
this->cloudMsg.set_pts_size(this->cloud_pts->length);
@@ -542,7 +542,7 @@
&angle_min, &angle_max, &angle_increment,
&range_max, &ranges_size , &ranges_alloc_size,
&intensities_size, &intensities_alloc_size,
- this->ranges , this->intensities) == PR2::PR2_ALL_OK)
+ this->ranges , this->intensities, &laserTime) == PR2::PR2_ALL_OK)
{
// Get latest laser data
this->laserMsg.angle_min = angle_min;
@@ -559,8 +559,8 @@
}
this->laserMsg.header.frame_id = tf.lookup("FRAMEID_LASER");
- this->laserMsg.header.stamp.sec = (unsigned long)floor(this->simTime);
- this->laserMsg.header.stamp.nsec = (unsigned long)floor( 1e9 * ( this->simTime - this->laserMsg.header.stamp.sec) );
+ this->laserMsg.header.stamp.sec = (unsigned long)floor(this->laserTime);
+ this->laserMsg.header.stamp.nsec = (unsigned long)floor( 1e9 * ( this->laserTime - this->laserMsg.header.stamp.sec) );
//publish("laser",this->laserMsg); // for laser_view FIXME: can alias this at the commandline or launch script
publish("scan",this->laserMsg); // for rosstage
@@ -640,7 +640,7 @@
&width , &height ,
&depth ,
&compression , &colorspace ,
- &buf_size , buf_ptz_right )) {
+ &buf_size , buf_ptz_right , &cameraTime)) {
this->img_ptz_right.width = width;
this->img_ptz_right.height = height;
this->img_ptz_right.compression = compression;
@@ -648,6 +648,10 @@
if(buf_size >0)
{
+ this->img_ptz_right.header.frame_id = tf.lookup("FRAMEID_PTZ_CAM_R");
+ this->img_ptz_right.header.stamp.sec = (unsigned long)floor(this->cameraTime);
+ this->img_ptz_right.header.stamp.nsec = (unsigned long)floor( 1e9 * ( this->cameraTime - this->laserMsg.header.stamp.sec) );
+
this->img_ptz_right.set_data_size(buf_size);
this->img_ptz_right.data = buf_ptz_right;
@@ -662,7 +666,7 @@
&width , &height ,
&depth ,
&compression , &colorspace ,
- &buf_size , buf_ptz_left )) {
+ &buf_size , buf_ptz_left , &cameraTime)) {
this->img_ptz_left .width = width;
this->img_ptz_left .height = height;
this->img_ptz_left .compression = compression;
@@ -670,6 +674,10 @@
if(buf_size >0)
{
+ this->img_ptz_left.header.frame_id = tf.lookup("FRAMEID_PTZ_CAM_L");
+ this->img_ptz_left.header.stamp.sec = (unsigned long)floor(this->cameraTime);
+ this->img_ptz_left.header.stamp.nsec = (unsigned long)floor( 1e9 * ( this->cameraTime - this->laserMsg.header.stamp.sec) );
+
this->img_ptz_left .set_data_size(buf_size);
this->img_ptz_left .data = buf_ptz_left ;
@@ -684,7 +692,7 @@
&width , &height ,
&depth ,
&compression , &colorspace ,
- &buf_size , buf_wrist_right )) {
+ &buf_size , buf_wrist_right , &cameraTime )) {
this->img_wrist_right.width = width;
this->img_wrist_right.height = height;
this->img_wrist_right.compression = compression;
@@ -692,6 +700,10 @@
if(buf_size >0)
{
+ this->img_wrist_right.header.frame_id = tf.lookup("FRAMEID_WRIST_CAM_R");
+ this->img_wrist_right.header.stamp.sec = (unsigned long)floor(this->cameraTime);
+ this->img_wrist_right.header.stamp.nsec = (unsigned long)floor( 1e9 * ( this->cameraTime - this->laserMsg.header.stamp.sec) );
+
this->img_wrist_right.set_data_size(buf_size);
this->img_wrist_right.data = buf_wrist_right;
@@ -706,7 +718,7 @@
&width , &height ,
&depth ,
&compression , &colorspace ,
- &buf_size , buf_wrist_left )) {
+ &buf_size , buf_wrist_left , &cameraTime )) {
this->img_wrist_left .width = width;
this->img_wrist_left .height = height;
this->img_wrist_left .compression = compression;
@@ -714,6 +726,10 @@
if(buf_size >0)
{
+ this->img_wrist_left.header.frame_id = tf.lookup("FRAMEID_WRIST_CAM_L");
+ this->img_wrist_left.header.stamp.sec = (unsigned long)floor(this->cameraTime);
+ this->img_wrist_left.header.stamp.nsec = (unsigned long)floor( 1e9 * ( this->cameraTime - this->laserMsg.header.stamp.sec) );
+
this->img_wrist_left .set_data_size(buf_size);
this->img_wrist_left .data = buf_wrist_left ;
@@ -728,7 +744,7 @@
&width , &height ,
&depth ,
&compression , &colorspace ,
- &buf_size , buf_forearm_right )) {
+ &buf_size , buf_forearm_right , &cameraTime )) {
this->img_forearm_right.width = width;
this->img_forearm_right.height = height;
this->img_forearm_right.compression = compression;
@@ -736,6 +752,10 @@
if(buf_size >0)
{
+ this->img_forearm_right.header.frame_id = tf.lookup("FRAMEID_FOREARM_CAM_R");
+ this->img_forearm_right.header.stamp.sec = (unsigned long)floor(this->cameraTime);
+ this->img_forearm_right.header.stamp.nsec = (unsigned long)floor( 1e9 * ( this->cameraTime - this->laserMsg.header.stamp.sec) );
+
this->img_forearm_right.set_data_size(buf_size);
this->img_forearm_right.data = buf_forearm_right;
@@ -750,7 +770,7 @@
&width , &height ,
&depth ,
&compression , &colorspace ,
- &buf_size , buf_forearm_left )) {
+ &buf_size , buf_forearm_left , &cameraTime )) {
this->img_forearm_left .width = width;
this->img_forearm_left .height = height;
this->img_forearm_left .compression = compression;
@@ -758,6 +778,10 @@
if(buf_size >0)
{
+ this->img_forearm_left.header.frame_id = tf.lookup("FRAMEID_FOREARM_CAM_L");
+ this->img_forearm_left.header.stamp.sec = (unsigned long)floor(this->cameraTime);
+ this->img_forearm_left.header.stamp.nsec = (unsigned long)floor( 1e9 * ( this->cameraTime - this->laserMsg.header.stamp.sec) );
+
this->img_forearm_left .set_data_size(buf_size);
this->img_forearm_left .data = buf_forearm_left ;
@@ -795,8 +819,8 @@
dAngle = -dAngle;
this->full_cloudMsg.header.frame_id = tf.lookup("FRAMEID_BASE");
- this->full_cloudMsg.header.stamp.sec = (unsigned long)floor(this->simTime);
- this->full_cloudMsg.header.stamp.nsec = (unsigned long)floor( 1e9 * ( this->simTime - this->full_cloudMsg.header.stamp.sec) );
+ this->full_cloudMsg.header.stamp.sec = (unsigned long)floor(this->laserTime);
+ this->full_cloudMsg.header.stamp.nsec = (unsigned long)floor( 1e9 * ( this->laserTime - this->full_cloudMsg.header.stamp.sec) );
int num_channels = 1;
this->full_cloudMsg.set_pts_size(this->full_cloud_pts->size());
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <adv...@us...> - 2008-07-28 23:38:25
|
Revision: 2214
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=2214&view=rev
Author: advaitjain
Date: 2008-07-28 23:38:31 +0000 (Mon, 28 Jul 2008)
Log Message:
-----------
Changed interpolation and IK to save the previous guess
within the pr2_kinematics class. This removes the need
for passing an initial guess configuration.
Also changed interpolated_kinematic_controller to use result
of previous IK as the new guess rather than reading the joint
angles.
Modified Paths:
--------------
pkg/trunk/drivers/robot/pr2/libpr2API/CMakeLists.txt
pkg/trunk/drivers/robot/pr2/libpr2API/include/libpr2API/pr2API.h
pkg/trunk/drivers/robot/pr2/libpr2API/src/pr2API.cpp
pkg/trunk/drivers/robot/pr2/libpr2API/src/test/pick_object.cpp
pkg/trunk/drivers/robot/pr2/libpr2API/src/test/pr2_kin_kdl.cpp
pkg/trunk/manip/teleop_arm_keyboard/teleop_arm_keyboard.cc
pkg/trunk/robot_descriptions/gazebo_robot_description/world/pr2.model
pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot.world
pkg/trunk/simulators/rosgazebo/include/RosGazeboNode/RosGazeboNode.h
pkg/trunk/simulators/rosgazebo/src/RosGazeboNode.cpp
pkg/trunk/util/kinematics/interpolated_kinematic_controller/src/interpolated_kinematic_controller.cc
pkg/trunk/util/kinematics/libKDL/CMakeLists.txt
pkg/trunk/util/kinematics/libKDL/include/libKDL/kdl_kinematics.h
pkg/trunk/util/kinematics/libKDL/src/kdl_kinematics.cpp
pkg/trunk/util/kinematics/libKDL/test/pr2_kin_test.cpp
Added Paths:
-----------
pkg/trunk/drivers/robot/pr2/libpr2API/src/old/
pkg/trunk/drivers/robot/pr2/libpr2API/src/old/kinematic_controllers.cpp
pkg/trunk/drivers/robot/pr2/libpr2API/src/old/test_kin_controllers.cpp
Removed Paths:
-------------
pkg/trunk/drivers/robot/pr2/libpr2API/src/kinematic_controllers.cpp
pkg/trunk/drivers/robot/pr2/libpr2API/src/test/test_kin_controllers.cpp
Modified: pkg/trunk/drivers/robot/pr2/libpr2API/CMakeLists.txt
===================================================================
--- pkg/trunk/drivers/robot/pr2/libpr2API/CMakeLists.txt 2008-07-28 23:37:45 UTC (rev 2213)
+++ pkg/trunk/drivers/robot/pr2/libpr2API/CMakeLists.txt 2008-07-28 23:38:31 UTC (rev 2214)
@@ -10,8 +10,11 @@
target_link_libraries(torque_test pr2API)
rospack_add_executable(pick_object src/test/pick_object.cpp)
target_link_libraries(pick_object pr2API)
-rospack_add_executable(test_kin_controllers src/test/test_kin_controllers.cpp src/kinematic_controllers.cpp)
-target_link_libraries(test_kin_controllers pr2API)
+
+# kinematic_controllers is now deprecated.
+#rospack_add_executable(test_kin_controllers src/test/test_kin_controllers.cpp src/kinematic_controllers.cpp)
+#target_link_libraries(test_kin_controllers pr2API)
+
rospack_add_executable(pr2_kin_kdl src/test/pr2_kin_kdl.cpp)
target_link_libraries(pr2_kin_kdl pr2API)
rospack_add_executable(test_FK src/test/test_FK.cpp)
Modified: pkg/trunk/drivers/robot/pr2/libpr2API/include/libpr2API/pr2API.h
===================================================================
--- pkg/trunk/drivers/robot/pr2/libpr2API/include/libpr2API/pr2API.h 2008-07-28 23:37:45 UTC (rev 2213)
+++ pkg/trunk/drivers/robot/pr2/libpr2API/include/libpr2API/pr2API.h 2008-07-28 23:38:31 UTC (rev 2214)
@@ -412,7 +412,12 @@
// KDL version of SetArmCartesianPosition
- public: PR2_ERROR_CODE SetArmCartesianPosition(PR2_MODEL_ID id, const KDL::Frame &f,const KDL::JntArray &q_init, KDL::JntArray &q_out);
+ public: PR2_ERROR_CODE SetArmCartesianPosition(PR2_MODEL_ID id, const KDL::Frame &f);
+ /*
+ * Preserved right now (July 28,2008) for compatibility with controller code.
+ * We might want to get rid of this later. -- Advait
+ */
+ PR2_ERROR_CODE SetArmCartesianPosition(PR2_MODEL_ID id, const KDL::Frame &f, const KDL::JntArray &q_init, KDL::JntArray &q_out);
/*! \fn
\brief Get the commanded position and speed for the end-effector
Deleted: pkg/trunk/drivers/robot/pr2/libpr2API/src/kinematic_controllers.cpp
===================================================================
--- pkg/trunk/drivers/robot/pr2/libpr2API/src/kinematic_controllers.cpp 2008-07-28 23:37:45 UTC (rev 2213)
+++ pkg/trunk/drivers/robot/pr2/libpr2API/src/kinematic_controllers.cpp 2008-07-28 23:38:31 UTC (rev 2214)
@@ -1,69 +0,0 @@
-/*
- Higher level controllers for the arm.
- I don't want to add to libpr2API and proper place for this code is yet to
- be decided.
-
-
-*/
-
-#include "kinematic_controllers.h"
-#include <kdl/rotational_interpolation_sa.hpp>
-
-#include <sys/time.h>
-#include <unistd.h>
-
-using namespace KDL;
-
-kinematic_controllers::kinematic_controllers()
-{
- this->myPR2 = new PR2::PR2Robot();
-}
-
-void kinematic_controllers::init()
-{
- this->myPR2->InitializeRobot();
-}
-
-void kinematic_controllers::go(KDL::Vector p, KDL::Rotation r, double step_size)
-{
- JntArray q = JntArray(this->myPR2->pr2_kin.nJnts);
- struct timeval t0, t1;
- gettimeofday(&t0,NULL);
-
- this->myPR2->GetArmJointPositionCmd(PR2::PR2_RIGHT_ARM, q);
- cout<<"current joint angles:"<<q<<endl;
-
- Frame f;
- this->myPR2->pr2_kin.FK(q,f);
- Vector start = f.p;
- Vector move = p-start;
- double dist = move.Norm();
- move = move/dist;
-//cout << "Start:"<<start<<", End:"<<p<<", dist: "<<dist<<", direction: "<<move<<endl;
-
- RotationalInterpolation_SingleAxis rotInterpolater;
- rotInterpolater.SetStartEnd(f.M, r);
- double total_angle = rotInterpolater.Angle();
-// printf("Angle: %f\n", rotInterpolater.Angle());
-
- Vector target;
- int nSteps = (int)(dist/step_size);
- double angle_step = total_angle/nSteps;
- for(int i=0;i<nSteps;i++)
- {
- f.p = start+(i+1)*move*step_size;
- f.M = rotInterpolater.Pos(angle_step*(i+1));
- this->myPR2->SetArmCartesianPosition(PR2::PR2_RIGHT_ARM,f,q,q);
- }
-
- f.p = p;
- f.M = r;
- this->myPR2->SetArmCartesianPosition(PR2::PR2_RIGHT_ARM,f,q,q);
- gettimeofday(&t1,NULL);
- double time_taken = (t1.tv_sec*1000000+t1.tv_usec - (t0.tv_sec*1000000+t0.tv_usec))/1000.;
- printf("Time taken to go: %f ms\n", time_taken);
-
-}
-
-
-
Copied: pkg/trunk/drivers/robot/pr2/libpr2API/src/old/kinematic_controllers.cpp (from rev 2066, pkg/trunk/drivers/robot/pr2/libpr2API/src/kinematic_controllers.cpp)
===================================================================
--- pkg/trunk/drivers/robot/pr2/libpr2API/src/old/kinematic_controllers.cpp (rev 0)
+++ pkg/trunk/drivers/robot/pr2/libpr2API/src/old/kinematic_controllers.cpp 2008-07-28 23:38:31 UTC (rev 2214)
@@ -0,0 +1,69 @@
+/*
+ Higher level controllers for the arm.
+ I don't want to add to libpr2API and proper place for this code is yet to
+ be decided.
+
+
+*/
+
+#include "kinematic_controllers.h"
+#include <kdl/rotational_interpolation_sa.hpp>
+
+#include <sys/time.h>
+#include <unistd.h>
+
+using namespace KDL;
+
+kinematic_controllers::kinematic_controllers()
+{
+ this->myPR2 = new PR2::PR2Robot();
+}
+
+void kinematic_controllers::init()
+{
+ this->myPR2->InitializeRobot();
+}
+
+void kinematic_controllers::go(KDL::Vector p, KDL::Rotation r, double step_size)
+{
+ JntArray q = JntArray(this->myPR2->pr2_kin.nJnts);
+ struct timeval t0, t1;
+ gettimeofday(&t0,NULL);
+
+ this->myPR2->GetArmJointPositionCmd(PR2::PR2_RIGHT_ARM, q);
+ cout<<"current joint angles:"<<q<<endl;
+
+ Frame f;
+ this->myPR2->pr2_kin.FK(q,f);
+ Vector start = f.p;
+ Vector move = p-start;
+ double dist = move.Norm();
+ move = move/dist;
+//cout << "Start:"<<start<<", End:"<<p<<", dist: "<<dist<<", direction: "<<move<<endl;
+
+ RotationalInterpolation_SingleAxis rotInterpolater;
+ rotInterpolater.SetStartEnd(f.M, r);
+ double total_angle = rotInterpolater.Angle();
+// printf("Angle: %f\n", rotInterpolater.Angle());
+
+ Vector target;
+ int nSteps = (int)(dist/step_size);
+ double angle_step = total_angle/nSteps;
+ for(int i=0;i<nSteps;i++)
+ {
+ f.p = start+(i+1)*move*step_size;
+ f.M = rotInterpolater.Pos(angle_step*(i+1));
+ this->myPR2->SetArmCartesianPosition(PR2::PR2_RIGHT_ARM,f,q,q);
+ }
+
+ f.p = p;
+ f.M = r;
+ this->myPR2->SetArmCartesianPosition(PR2::PR2_RIGHT_ARM,f,q,q);
+ gettimeofday(&t1,NULL);
+ double time_taken = (t1.tv_sec*1000000+t1.tv_usec - (t0.tv_sec*1000000+t0.tv_usec))/1000.;
+ printf("Time taken to go: %f ms\n", time_taken);
+
+}
+
+
+
Copied: pkg/trunk/drivers/robot/pr2/libpr2API/src/old/test_kin_controllers.cpp (from rev 2066, pkg/trunk/drivers/robot/pr2/libpr2API/src/test/test_kin_controllers.cpp)
===================================================================
--- pkg/trunk/drivers/robot/pr2/libpr2API/src/old/test_kin_controllers.cpp (rev 0)
+++ pkg/trunk/drivers/robot/pr2/libpr2API/src/old/test_kin_controllers.cpp 2008-07-28 23:38:31 UTC (rev 2214)
@@ -0,0 +1,133 @@
+
+#include "kinematic_controllers.h"
+#include <math.h>
+
+using namespace KDL;
+using namespace PR2;
+using namespace std;
+
+
+#include <termios.h>
+#include <string.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <unistd.h>
+
+int kfd = 0;
+struct termios cooked, raw;
+
+kinematic_controllers myKinCon;
+
+void top();
+void object_pose();
+void go_down();
+void close_gripper();
+void open_gripper();
+
+
+void keyboardLoop()
+{
+ char c;
+
+ // get the console in raw mode
+ tcgetattr(kfd, &cooked);
+ memcpy(&raw, &cooked, sizeof(struct termios));
+ raw.c_lflag &=~ (ICANON | ECHO);
+ raw.c_cc[VEOL] = 1;
+ raw.c_cc[VEOF] = 2;
+ tcsetattr(kfd, TCSANOW, &raw);
+
+ puts("Reading from keyboard");
+ puts("---------------------------");
+ puts("---------------------------");
+
+ for(;;)
+ {
+ // get the next event from the keyboard
+ if(read(kfd, &c, 1) < 0)
+ {
+ perror("read():");
+ exit(-1);
+ }
+
+ switch(c)
+ {
+ case '1':
+ printf("You pressed 1\n");
+ open_gripper();
+ top();
+ break;
+ case '2':
+ printf("You pressed 2\n");
+ object_pose();
+ break;
+ case '3':
+ printf("You pressed 3\n");
+ go_down();
+ break;
+ case '4':
+ printf("You pressed 4\n");
+ close_gripper();
+ break;
+ case '5':
+ printf("You pressed 4\n");
+ object_pose();
+ break;
+ case '6':
+ printf("You pressed 4\n");
+ top();
+ break;
+ default:
+ printf("You pressed Something Else\n");
+ break;
+ }
+ myKinCon.myPR2->hw.UpdateHW();
+ }
+}
+
+void top()
+{
+ Rotation r = Rotation::RotZ(DTOR(0));
+ Vector v(0.3,-0.2,0.56);
+ myKinCon.go(v,r,0.05);
+}
+
+void object_pose()
+{
+ Rotation r = Rotation::RotZ(DTOR(90));
+ Vector v(0.578,0.01,0.06);
+ myKinCon.go(v,r,0.05);
+}
+
+void go_down()
+{
+ Rotation r = Rotation::RotZ(DTOR(90))*Rotation::RotY(DTOR(30));
+ Vector v(0.578,0.01,-0.01);
+ myKinCon.go(v,r,0.05);
+}
+
+void close_gripper()
+{
+ myKinCon.myPR2->hw.CloseGripper(PR2::PR2_RIGHT_GRIPPER, 0.0, 10000);
+}
+
+void open_gripper()
+{
+ myKinCon.myPR2->hw.CloseGripper(PR2::PR2_RIGHT_GRIPPER, 0.15, 500);
+}
+
+void init_robot()
+{
+ myKinCon.init();
+}
+
+
+
+int main(int argc, char **argv)
+{
+ init_robot();
+ keyboardLoop();
+ return 0;
+}
+
+
Modified: pkg/trunk/drivers/robot/pr2/libpr2API/src/pr2API.cpp
===================================================================
--- pkg/trunk/drivers/robot/pr2/libpr2API/src/pr2API.cpp 2008-07-28 23:37:45 UTC (rev 2213)
+++ pkg/trunk/drivers/robot/pr2/libpr2API/src/pr2API.cpp 2008-07-28 23:38:31 UTC (rev 2214)
@@ -149,7 +149,10 @@
return PR2_ALL_OK;
}
-
+/*
+ * Preserved right now (July 28,2008) for compatibility with controller code.
+ * We might want to get rid of this later. -- Advait
+ */
PR2_ERROR_CODE PR2Robot::SetArmCartesianPosition(PR2_MODEL_ID id, const KDL::Frame &f, const KDL::JntArray &q_init, KDL::JntArray &q_out)
{
// KDL::JntArray q_init = KDL::JntArray(this->pr2_kin.nJnts);
@@ -177,6 +180,36 @@
return PR2_ALL_OK;
}
+PR2_ERROR_CODE PR2Robot::SetArmCartesianPosition(PR2_MODEL_ID id, const KDL::Frame &f)
+{
+ if (this->pr2_kin.IK(f) == true)
+ {
+// cout<<"IK guess:"<<*this->pr2_kin.q_IK_guess<<endl;
+// cout<<"IK result:"<<*this->pr2_kin.q_IK_result<<endl;
+ }
+ else
+ cout<<"Could not compute Inv Kin."<<endl;
+
+ //------ checking that IK returned a valid soln -----
+ KDL::Frame f_ik;
+ if (this->pr2_kin.FK(*this->pr2_kin.q_IK_result,f_ik))
+ {
+ // cout<<"End effector after IK:"<<f_ik<<endl;
+ }
+ else
+ cout<<"Could not compute Fwd Kin. (After IK)"<<endl;
+
+ for(int ii = 0; ii < 7; ii++)
+ hw.SetJointServoCmd((PR2::PR2_JOINT_ID) (JointStart[id]+ii),(*this->pr2_kin.q_IK_result)(ii),0);
+
+ KDL::JntArray *t = this->pr2_kin.q_IK_result;
+ this->pr2_kin.q_IK_result = this->pr2_kin.q_IK_guess;
+ this->pr2_kin.q_IK_guess = t; // update guess with the computed IK result.
+
+
+ return PR2_ALL_OK;
+}
+
PR2_ERROR_CODE PR2Robot::SetArmCartesianPosition(PR2_MODEL_ID id, NEWMAT::Matrix g)
{
NEWMAT::Matrix theta(8,8);
Modified: pkg/trunk/drivers/robot/pr2/libpr2API/src/test/pick_object.cpp
===================================================================
--- pkg/trunk/drivers/robot/pr2/libpr2API/src/test/pick_object.cpp 2008-07-28 23:37:45 UTC (rev 2213)
+++ pkg/trunk/drivers/robot/pr2/libpr2API/src/test/pick_object.cpp 2008-07-28 23:38:31 UTC (rev 2214)
@@ -18,10 +18,10 @@
struct termios cooked, raw;
PR2::PR2Robot myPR2;
-JntArray object_pose();
+void object_pose();
void open_gripper();
void close_gripper();
-void go_down(const JntArray &guess);
+void go_down();
void keyboardLoop()
@@ -40,7 +40,6 @@
puts("---------------------------");
puts("---------------------------");
- JntArray q = JntArray(myPR2.pr2_kin.nJnts);
for(;;)
{
// get the next event from the keyboard
@@ -58,11 +57,11 @@
break;
case '2':
printf("You pressed 2\n");
- q = object_pose();
+ object_pose();
break;
case '3':
printf("You pressed 3\n");
- go_down(q);
+ go_down();
break;
case '4':
printf("You pressed 4\n");
@@ -91,12 +90,13 @@
Frame f;
myPR2.pr2_kin.FK(pr2_config,f);
// send command to robot
- JntArray q_init = JntArray(myPR2.pr2_kin.nJnts);
- q_init(0) = 0.0, q_init(1) = 0.0, q_init(2) = 0.0, q_init(3) = 0.0;
- q_init(4) = 0.0, q_init(5) = 0.0, q_init(6) = 0.0;
- JntArray q_out = JntArray(myPR2.pr2_kin.nJnts);
- myPR2.SetArmCartesianPosition(PR2::PR2_RIGHT_ARM,f,q_init,q_out);
+ (*myPR2.pr2_kin.q_IK_guess)(0) = 0.0, (*myPR2.pr2_kin.q_IK_guess)(1) = 0.0, (*myPR2.pr2_kin.q_IK_guess)(2) = 0.0;
+ (*myPR2.pr2_kin.q_IK_guess)(3) = 0.0, (*myPR2.pr2_kin.q_IK_guess)(4) = 0.0, (*myPR2.pr2_kin.q_IK_guess)(5) = 0.0;
+ (*myPR2.pr2_kin.q_IK_guess)(6) = 0.0;
+
+ myPR2.SetArmCartesianPosition(PR2::PR2_RIGHT_ARM,f);
+
}
void close_gripper()
@@ -109,16 +109,17 @@
myPR2.hw.CloseGripper(PR2::PR2_RIGHT_GRIPPER, 0.15, 500);
}
-void go_down(const JntArray &guess)
+void go_down()
{
+ // called after object_pose so q_IK_guess should already be correct.
Rotation r = Rotation::RotZ(DTOR(90));
Vector v(0.568,0.01,-0.01);
Frame f(r,v);
- JntArray q_out = JntArray(myPR2.pr2_kin.nJnts);
- myPR2.SetArmCartesianPosition(PR2::PR2_RIGHT_ARM,f,guess,q_out);
+
+ myPR2.SetArmCartesianPosition(PR2::PR2_RIGHT_ARM,f);
}
-JntArray object_pose()
+void object_pose()
{
Rotation r = Rotation::RotZ(DTOR(90));
@@ -126,41 +127,16 @@
Vector v(0.568,0.01,0.06);
Frame f(r,v);
- JntArray q_init = JntArray(myPR2.pr2_kin.nJnts);
- q_init(0) = DTOR(-30); // turret
- q_init(1) = DTOR(-20); // shoulder pitch
- q_init(2) = DTOR(60); // shoulder roll
- q_init(3) = DTOR(60); // elbow pitch
- q_init(4) = DTOR(20); // elbow roll
- q_init(5) = DTOR(30); // wrist pitch
- q_init(6) = 0.0; // wrist roll
-// myPR2.pr2_kin.FK(q_init,f);
-// cout<<"end effector frame: "<<f<<endl;
+ (*myPR2.pr2_kin.q_IK_guess)(0) = DTOR(-20); // turret
+ (*myPR2.pr2_kin.q_IK_guess)(1) = DTOR(-20); // shoulder pitch
+ (*myPR2.pr2_kin.q_IK_guess)(2) = DTOR(40); // shoulder roll
+ (*myPR2.pr2_kin.q_IK_guess)(3) = DTOR(60); // elbow pitch
+ (*myPR2.pr2_kin.q_IK_guess)(4) = DTOR(20); // elbow roll
+ (*myPR2.pr2_kin.q_IK_guess)(5) = DTOR(30); // wrist pitch
+ (*myPR2.pr2_kin.q_IK_guess)(6) = 0.0; // wrist roll
- q_init(0) = DTOR(-20); // turret
- q_init(1) = DTOR(-20); // shoulder pitch
- q_init(2) = DTOR(40); // shoulder roll
- q_init(3) = DTOR(60); // elbow pitch
- q_init(4) = DTOR(20); // elbow roll
- q_init(5) = DTOR(30); // wrist pitch
- q_init(6) = 0.0; // wrist roll
-
- JntArray q_out = JntArray(myPR2.pr2_kin.nJnts);
- myPR2.SetArmCartesianPosition(PR2::PR2_RIGHT_ARM,f,q_init,q_out);
- return q_out;
-
-/*
- end effector frame: [[ +0.592488, +0.135547, +0.794094;
- +0.741583, +0.293264, -0.603367;
- -0.314663, +0.946374, +0.0732355]
- [ +0.698411, -0.130820, +0.0626092]]
-*/
-
-// double jntArr[7] = {q_init(0),q_init(1),q_init(2),q_init(3),q_init(4),q_init(5),q_init(6)};
-// double jntSpd[7] = {0,0,0,0,0,0,0};
-// myPR2.SetArmJointPosition(PR2::PR2_RIGHT_ARM, jntArr,jntSpd);
-
+ myPR2.SetArmCartesianPosition(PR2::PR2_RIGHT_ARM,f);
}
Modified: pkg/trunk/drivers/robot/pr2/libpr2API/src/test/pr2_kin_kdl.cpp
===================================================================
--- pkg/trunk/drivers/robot/pr2/libpr2API/src/test/pr2_kin_kdl.cpp 2008-07-28 23:37:45 UTC (rev 2213)
+++ pkg/trunk/drivers/robot/pr2/libpr2API/src/test/pr2_kin_kdl.cpp 2008-07-28 23:38:31 UTC (rev 2214)
@@ -56,11 +56,10 @@
cout<<"Could not compute Fwd Kin."<<endl;
// send command to robot
- JntArray q_init = JntArray(myPR2.pr2_kin.nJnts);
- JntArray q_out = JntArray(myPR2.pr2_kin.nJnts);
- q_init(0) = 0.0, q_init(1) = 0.0, q_init(2) = 0.0, q_init(3) = 0.0;
- q_init(4) = 0.0, q_init(5) = 0.0, q_init(6) = 0.0;
- myPR2.SetArmCartesianPosition(PR2::PR2_RIGHT_ARM,f,q_init,q_out);
+ (*myPR2.pr2_kin.q_IK_guess)(0) = 0.0, (*myPR2.pr2_kin.q_IK_guess)(1) = 0.0, (*myPR2.pr2_kin.q_IK_guess)(2) = 0.0;
+ (*myPR2.pr2_kin.q_IK_guess)(3) = 0.0, (*myPR2.pr2_kin.q_IK_guess)(4) = 0.0, (*myPR2.pr2_kin.q_IK_guess)(5) = 0.0;
+ (*myPR2.pr2_kin.q_IK_guess)(6) = 0.0;
+ myPR2.SetArmCartesianPosition(PR2::PR2_RIGHT_ARM,f);
myPR2.GetBasePositionActual(&x, &y, &z, &roll, &pitch, &yaw);
Deleted: pkg/trunk/drivers/robot/pr2/libpr2API/src/test/test_kin_controllers.cpp
===================================================================
--- pkg/trunk/drivers/robot/pr2/libpr2API/src/test/test_kin_controllers.cpp 2008-07-28 23:37:45 UTC (rev 2213)
+++ pkg/trunk/drivers/robot/pr2/libpr2API/src/test/test_kin_controllers.cpp 2008-07-28 23:38:31 UTC (rev 2214)
@@ -1,133 +0,0 @@
-
-#include "kinematic_controllers.h"
-#include <math.h>
-
-using namespace KDL;
-using namespace PR2;
-using namespace std;
-
-
-#include <termios.h>
-#include <string.h>
-#include <stdio.h>
-#include <stdlib.h>
-#include <unistd.h>
-
-int kfd = 0;
-struct termios cooked, raw;
-
-kinematic_controllers myKinCon;
-
-void top();
-void object_pose();
-void go_down();
-void close_gripper();
-void open_gripper();
-
-
-void keyboardLoop()
-{
- char c;
-
- // get the console in raw mode
- tcgetattr(kfd, &cooked);
- memcpy(&raw, &cooked, sizeof(struct termios));
- raw.c_lflag &=~ (ICANON | ECHO);
- raw.c_cc[VEOL] = 1;
- raw.c_cc[VEOF] = 2;
- tcsetattr(kfd, TCSANOW, &raw);
-
- puts("Reading from keyboard");
- puts("---------------------------");
- puts("---------------------------");
-
- for(;;)
- {
- // get the next event from the keyboard
- if(read(kfd, &c, 1) < 0)
- {
- perror("read():");
- exit(-1);
- }
-
- switch(c)
- {
- case '1':
- printf("You pressed 1\n");
- open_gripper();
- top();
- break;
- case '2':
- printf("You pressed 2\n");
- object_pose();
- break;
- case '3':
- printf("You pressed 3\n");
- go_down();
- break;
- case '4':
- printf("You pressed 4\n");
- close_gripper();
- break;
- case '5':
- printf("You pressed 4\n");
- object_pose();
- break;
- case '6':
- printf("You pressed 4\n");
- top();
- break;
- default:
- printf("You pressed Something Else\n");
- break;
- }
- myKinCon.myPR2->hw.UpdateHW();
- }
-}
-
-void top()
-{
- Rotation r = Rotation::RotZ(DTOR(0));
- Vector v(0.3,-0.2,0.56);
- myKinCon.go(v,r,0.05);
-}
-
-void object_pose()
-{
- Rotation r = Rotation::RotZ(DTOR(90));
- Vector v(0.578,0.01,0.06);
- myKinCon.go(v,r,0.05);
-}
-
-void go_down()
-{
- Rotation r = Rotation::RotZ(DTOR(90))*Rotation::RotY(DTOR(30));
- Vector v(0.578,0.01,-0.01);
- myKinCon.go(v,r,0.05);
-}
-
-void close_gripper()
-{
- myKinCon.myPR2->hw.CloseGripper(PR2::PR2_RIGHT_GRIPPER, 0.0, 10000);
-}
-
-void open_gripper()
-{
- myKinCon.myPR2->hw.CloseGripper(PR2::PR2_RIGHT_GRIPPER, 0.15, 500);
-}
-
-void init_robot()
-{
- myKinCon.init();
-}
-
-
-
-int main(int argc, char **argv)
-{
- init_robot();
- keyboardLoop();
- return 0;
-}
-
-
Modified: pkg/trunk/manip/teleop_arm_keyboard/teleop_arm_keyboard.cc
===================================================================
--- pkg/trunk/manip/teleop_arm_keyboard/teleop_arm_keyboard.cc 2008-07-28 23:37:45 UTC (rev 2213)
+++ pkg/trunk/manip/teleop_arm_keyboard/teleop_arm_keyboard.cc 2008-07-28 23:38:31 UTC (rev 2214)
@@ -260,10 +260,12 @@
}
if(jointID == ARM_R_GRIPPER) {
this->cmd_rightarmconfig.gripperForceCmd = 50;
- this->cmd_rightarmconfig.gripperGapCmd = .1;
+ this->cmd_rightarmconfig.gripperGapCmd = .2;
+ printf("Opening right gripper\n");
} else {
this->cmd_leftarmconfig.gripperForceCmd = 50;
- this->cmd_leftarmconfig.gripperGapCmd = .1;
+ this->cmd_leftarmconfig.gripperGapCmd = .2;
+ printf("Opening left gripper\n");
}
}
@@ -391,53 +393,52 @@
}
switch(c)
- {
- case 'l':
- case 'L':
- right_arm = false;
- printf("Actuating left arm.\n");
- break;
- case 'r':
- case 'R':
- right_arm = true;
- printf("Actuating right arm.\n");
- break;
- case '+':
- case '=':
- changeJointAngle(curr_jointID, true);
- dirty=true;
- break;
- case '_':
- case '-':
- changeJointAngle(curr_jointID, false);
- dirty=true;
- break;
- case '.':
- _rightInit = false;
- _leftInit = false;
- sleep(1);
- openGripper(curr_jointID);
- dirty = true;
- break;
- case '/':
- _rightInit = false;
- _leftInit = false;
- sleep(1);
- closeGripper(curr_jointID);
- dirty = true;
- break;
- case 'q':
- printCurrentJointValues();
- break;
- case 'k':
- printCurrentEndEffectorWorldCoord();
- break;
- case 'j':
- printCurrentEndEffectorShoulderCoord();
- break;
- default:
- break;
- }
+ {
+ case 'l':
+ case 'L':
+ right_arm = false;
+ printf("Actuating left arm.\n");
+ break;
+ case 'r':
+ case 'R':
+ right_arm = true;
+ printf("Actuating right arm.\n");
+ break;
+ case '+':
+ case '=':
+ changeJointAngle(curr_jointID, true);
+ dirty=true;
+ break;
+ case '_':
+ case '-':
+ changeJointAngle(curr_jointID, false);
+ dirty=true;
+ break;
+ case '.':
+ _rightInit = false;
+ _leftInit = false;
+ openGripper(curr_jointID);
+ dirty = true;
+ break;
+ case '/':
+ _rightInit = false;
+ _leftInit = false;
+ sleep(1);
+ closeGripper(curr_jointID);
+ dirty = true;
+ break;
+ case 'q':
+ printCurrentJointValues();
+ break;
+ case 'k':
+ printCurrentEndEffectorWorldCoord();
+ break;
+ case 'j':
+ printCurrentEndEffectorShoulderCoord();
+ break;
+ default:
+ break;
+ }
if (right_arm==false)
{
Modified: pkg/trunk/robot_descriptions/gazebo_robot_description/world/pr2.model
===================================================================
--- pkg/trunk/robot_descriptions/gazebo_robot_description/world/pr2.model 2008-07-28 23:37:45 UTC (rev 2213)
+++ pkg/trunk/robot_descriptions/gazebo_robot_description/world/pr2.model 2008-07-28 23:38:31 UTC (rev 2214)
@@ -432,26 +432,9 @@
<material>Gazebo/PioneerBody</material>
</visual>
</geom:cylinder>
- <sensor:camera name="wrist_cam_right_sensor">
- <imageSize>640 480</imageSize>
- <hfov>60</hfov>
- <nearClip>0.1</nearClip>
- <farClip>100</farClip>
- <updateRate>15.0</updateRate>
- <!--
- <controller:ros_camera name="wrist_cam_right_controller" plugin="libRos_Camera.so">
- <updateRate>15.0</updateRate>
- <topicName>image_wrist_cam_right</topicName>
- <interface:camera name="wrist_cam_right_iface" />
- </controller:ros_camera>
- -->
- <controller:generic_camera name="wrist_cam_right_controller">
- <updateRate>15.0</updateRate>
- <interface:camera name="wrist_cam_right_iface" />
- </controller:generic_camera>
- </sensor:camera>
</body:cylinder>
+
<body:box name="palm_right">
<xyz> 1.10 -0.15 0.8269</xyz>
<rpy> 0.0 0.0 0.0 </rpy>
@@ -482,6 +465,43 @@
</geom:box>
</body:box>
+ <body:empty name="palm_cam_right_body">
+ <xyz> 1.10 -0.15 0.8569</xyz>
+ <rpy> 0.0 0.0 0.0 </rpy>
+ <sensor:camera name="palm_cam_right_sensor">
+ <imageSize>640 480</imageSize>
+ <hfov>60</hfov>
+ <nearClip>0.1</nearClip>
+ <farClip>100</farClip>
+ <updateRate>15.0</updateRate>
+ <controller:generic_camera name="wrist_cam_right_controller">
+ <updateRate>15.0</updateRate>
+ <interface:camera name="wrist_cam_right_iface" />
+ </controller:generic_camera>
+ </sensor:camera>
+ <geom:box name="palm_cam_right_geom">
+ <xyz> 0.00 0.00 0.00</xyz>
+ <size>0.02 0.02 0.02</size>
+ <mass>0.1</mass>
+ <visual>
+ <size>0.02 0.02 0.02</size>
+ <rpy>0.0 0.0 0.0 </rpy>
+ <mesh>unit_box</mesh>
+ <material>Gazebo/Black</material>
+ </visual>
+ </geom:box>
+ </body:empty>
+ <joint:hinge name="palm_cam_right">
+ <body1>palm_cam_right_body</body1>
+ <body2>palm_right</body2>
+ <anchor>palm_right</anchor>
+ <axis> 0.0 0.0 1.0 </axis>
+ <lowStop> 0.0 </lowStop>
+ <highStop> 0.0 </highStop>
+ </joint:hinge>
+
+
+
<body:box name="finger_1_right">
<xyz> 1.18 -0.17 0.8269 </xyz>
<rpy> 0.0 0.0 0.0 </rpy>
Modified: pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot.world
===================================================================
--- pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot.world 2008-07-28 23:37:45 UTC (rev 2213)
+++ pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot.world 2008-07-28 23:38:31 UTC (rev 2214)
@@ -81,18 +81,18 @@
<!-- The "desk"-->
<model:physical name="desk1_model">
- <xyz> 2.28 -0.21 -0.10</xyz>
+ <xyz> 0.80 -0.21 -0.10</xyz>
<rpy> 0.0 0.0 0.00</rpy>
<body:box name="desk1_legs_body">
<geom:box name="desk1_legs_geom">
<kp>100000000.0</kp>
<kd>1.0</kd>
- <xyz> 0.0 0.0 0.50</xyz>
+ <xyz> 0.0 0.0 0.60</xyz>
<mesh>default</mesh>
- <size>0.5 1.0 0.75</size>
+ <size>0.5 1.0 0.6</size>
<mass> 10.0</mass>
<visual>
- <size> 0.5 1.0 0.75</size>
+ <size> 0.5 1.0 0.60</size>
<material>Gazebo/Rocky</material>
<mesh>unit_box</mesh>
</visual>
Modified: pkg/trunk/simulators/rosgazebo/include/RosGazeboNode/RosGazeboNode.h
===================================================================
--- pkg/trunk/simulators/rosgazebo/include/RosGazeboNode/RosGazeboNode.h 2008-07-28 23:37:45 UTC (rev 2213)
+++ pkg/trunk/simulators/rosgazebo/include/RosGazeboNode/RosGazeboNode.h 2008-07-28 23:38:31 UTC (rev 2214)
@@ -153,6 +153,10 @@
void cmd_leftarmcartesianReceived();
void cmd_rightarmcartesianReceived();
+ // Message callback which sets pr2_kin.q_IK_guess to the current manipulator configuration.
+ // July 24, 2008 - Advait - only right arm is supported
+ void reset_IK_guess();
+
// laser range data
float ranges[GZ_LASER_MAX_RANGES];
uint8_t intensities[GZ_LASER_MAX_RANGES];
@@ -175,6 +179,8 @@
pr2_msgs::EndEffectorState cmd_leftarmcartesian;
pr2_msgs::EndEffectorState cmd_rightarmcartesian;
+ std_msgs::Empty empty_msg;
+
//Flags to indicate that a new message has arrived
bool newRightArmPos;
bool newLeftArmPos;
Modified: pkg/trunk/simulators/rosgazebo/src/RosGazeboNode.cpp
===================================================================
--- pkg/trunk/simulators/rosgazebo/src/RosGazeboNode.cpp 2008-07-28 23:37:45 UTC (rev 2213)
+++ pkg/trunk/simulators/rosgazebo/src/RosGazeboNode.cpp 2008-07-28 23:38:31 UTC (rev 2214)
@@ -117,13 +117,18 @@
f.p.data[i] = cmd_leftarmcartesian.trans[i];
}
- KDL::JntArray q = KDL::JntArray(this->PR2Copy->pr2_kin.nJnts);
- this->PR2Copy->GetArmJointPositionCmd(PR2::PR2_LEFT_ARM, q);
- this->PR2Copy->SetArmCartesianPosition(PR2::PR2_LEFT_ARM,f,q,q);
+// KDL::JntArray q = KDL::JntArray(this->PR2Copy->pr2_kin.nJnts);
+// this->PR2Copy->GetArmJointPositionCmd(PR2::PR2_LEFT_ARM, q);
+ this->PR2Copy->SetArmCartesianPosition(PR2::PR2_LEFT_ARM,f);
this->lock.unlock();
}
+void RosGazeboNode::reset_IK_guess()
+{
+ this->PR2Copy->GetArmJointPositionCmd(PR2::PR2_RIGHT_ARM, *(this->PR2Copy->pr2_kin.q_IK_guess));
+}
+
void RosGazeboNode::cmd_rightarmcartesianReceived() {
this->lock.lock();
@@ -135,9 +140,9 @@
f.p.data[i] = cmd_rightarmcartesian.trans[i];
}
- KDL::JntArray q = KDL::JntArray(this->PR2Copy->pr2_kin.nJnts);
- this->PR2Copy->GetArmJointPositionCmd(PR2::PR2_RIGHT_ARM, q);
- this->PR2Copy->SetArmCartesianPosition(PR2::PR2_RIGHT_ARM,f, q,q);
+// KDL::JntArray q = KDL::JntArray(this->PR2Copy->pr2_kin.nJnts);
+// this->PR2Copy->GetArmJointPositionCmd(PR2::PR2_RIGHT_ARM, q);
+ this->PR2Copy->SetArmCartesianPosition(PR2::PR2_RIGHT_ARM,f);
this->lock.unlock();
}
@@ -346,6 +351,7 @@
subscribe("cmd_rightarmconfig", rightarm, &RosGazeboNode::cmd_rightarmconfigReceived);
subscribe("cmd_leftarm_cartesian", cmd_leftarmcartesian, &RosGazeboNode::cmd_leftarmcartesianReceived...
[truncated message content] |
|
From: <hsu...@us...> - 2008-07-29 00:05:30
|
Revision: 2217
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=2217&view=rev
Author: hsujohnhsu
Date: 2008-07-29 00:05:38 +0000 (Tue, 29 Jul 2008)
Log Message:
-----------
name change for pr2 model.
Modified Paths:
--------------
pkg/trunk/drivers/robot/pr2/libpr2HW/src/pr2HW.cpp
pkg/trunk/robot_descriptions/gazebo_robot_description/world/pr2-test.model
pkg/trunk/robot_descriptions/gazebo_robot_description/world/pr2.model
pkg/trunk/robot_descriptions/gazebo_robot_description/world/pr2_new_plugins.model
pkg/trunk/robot_descriptions/gazebo_robot_description/world/pr2_torque.model
pkg/trunk/robot_descriptions/gazebo_robot_description/world/pr2_torque_control.model
pkg/trunk/robot_descriptions/gazebo_robot_description/world/pr2_torque_position.model
pkg/trunk/robot_descriptions/gazebo_robot_description/world/pr2_trimesh.model
Modified: pkg/trunk/drivers/robot/pr2/libpr2HW/src/pr2HW.cpp
===================================================================
--- pkg/trunk/drivers/robot/pr2/libpr2HW/src/pr2HW.cpp 2008-07-28 23:54:01 UTC (rev 2216)
+++ pkg/trunk/drivers/robot/pr2/libpr2HW/src/pr2HW.cpp 2008-07-29 00:05:38 UTC (rev 2217)
@@ -217,7 +217,7 @@
/// Open the laser interface for hokuyo
try
{
- pr2LaserIface->Open(client, "laser_iface_1");
+ pr2LaserIface->Open(client, "tilt_laser_iface_1");
}
catch (std::string e)
{
Modified: pkg/trunk/robot_descriptions/gazebo_robot_description/world/pr2-test.model
===================================================================
--- pkg/trunk/robot_descriptions/gazebo_robot_description/world/pr2-test.model 2008-07-28 23:54:01 UTC (rev 2216)
+++ pkg/trunk/robot_descriptions/gazebo_robot_description/world/pr2-test.model 2008-07-29 00:05:38 UTC (rev 2217)
@@ -137,7 +137,7 @@
<!--
-->
<!--
- <sensor:ray2 name="laser_1">
+ <sensor:ray2 name="base_laser_1">
<rayCount>683</rayCount>
<rangeCount>683</rangeCount>
<laserCount>1</laserCount>
@@ -148,8 +148,8 @@
<minRange>0.05</minRange>
<maxRange>10.0</maxRange>
- <controller:sick2lms200_laser name="laser_controller_1">
- <interface:laser name="laser_iface_1"/>
+ <controller:sick2lms200_laser name="base_laser_controller_1">
+ <interface:laser name="base_laser_iface_1"/>
<updateRate>10</updateRate>
</controller:sick2lms200_laser>
</sensor:ray2>
@@ -157,7 +157,7 @@
</body:box>
<!-- attach hokuyo to torso -->
- <joint:hinge name="tiltlaser_torso_attach_joint">
+ <joint:hinge name="base_laser_torso_attach_joint">
<body1>base_body</body1>
<body2>base_laser_body</body2>
<anchor>base_laser_body</anchor>
@@ -1123,11 +1123,11 @@
</joint:hinge>
<!-- Hokuyo laser body -->
- <body:box name="laser_body">
+ <body:box name="tilt_laser_body">
<xyz>0.23 0.0 1.0</xyz>
<rpy>0.0 0.0 0.0</rpy>
- <geom:box name="laser_box">
+ <geom:box name="tilt_laser_box">
<xyz>0.0 0.0 0.02</xyz>
<rpy>0 0 0</rpy>
<size>0.05 0.05 0.041</size>
@@ -1140,7 +1140,7 @@
</visual>
</geom:box>
- <geom:cylinder name="laser_cylinder1">
+ <geom:cylinder name="tilt_laser_cylinder1">
<xyz>0.0 0.0 0.041</xyz>
<rpy>0 0 0</rpy>
<size>0.02 0.013</size>
@@ -1155,7 +1155,7 @@
</geom:cylinder>
- <geom:cylinder name="laser_cylinder2">
+ <geom:cylinder name="tilt_laser_cylinder2">
<xyz>0.0 0.0 0.054</xyz>
<rpy>0 0 0</rpy>
<size>0.018 0.009</size>
@@ -1170,7 +1170,7 @@
</geom:cylinder>
- <geom:cylinder name="laser_cylinder3">
+ <geom:cylinder name="tilt_laser_cylinder3">
<xyz>0.0 0.0 0.063</xyz>
<rpy>0 0 0</rpy>
<size>0.0175 0.008</size>
@@ -1186,7 +1186,7 @@
</geom:cylinder>
- <sensor:ray name="laser_1">
+ <sensor:ray name="tilt_laser_1">
<rayCount>68</rayCount>
<rangeCount>683</rangeCount>
<laserCount>1</laserCount>
@@ -1198,15 +1198,15 @@
<minRange>0.05</minRange>
<maxRange>10.0</maxRange>
<updateRate>10</updateRate>
- <controller:sicklms200_laser name="laser_controller_1">
- <interface:laser name="laser_iface_1"/>
+ <controller:sicklms200_laser name="tilt_laser_controller_1">
+ <interface:laser name="tilt_laser_iface_1"/>
<updateRate>10</updateRate>
</controller:sicklms200_laser>
</sensor:ray>
<!--
-->
<!--
- <sensor:ray2 name="laser_1">
+ <sensor:ray2 name="tilt_laser_1">
<rayCount>683</rayCount>
<rangeCount>683</rangeCount>
<laserCount>1</laserCount>
@@ -1217,8 +1217,8 @@
<minRange>0.05</minRange>
<maxRange>10.0</maxRange>
- <controller:sick2lms200_laser name="laser_controller_1">
- <interface:laser name="laser_iface_1"/>
+ <controller:sick2lms200_laser name="tilt_laser_controller_1">
+ <interface:laser name="tilt_laser_iface_1"/>
<updateRate>10</updateRate>
</controller:sick2lms200_laser>
</sensor:ray2>
@@ -1229,8 +1229,8 @@
<!-- attach hokuyo to torso -->
<joint:hinge name="hokuyo_pitch_joint">
<body1>neck_body</body1>
- <body2>laser_body</body2>
- <anchor>laser_body</anchor>
+ <body2>tilt_laser_body</body2>
+ <anchor>tilt_laser_body</anchor>
<axis> 0.0 1.0 0.0 </axis>
</joint:hinge>
Modified: pkg/trunk/robot_descriptions/gazebo_robot_description/world/pr2.model
===================================================================
--- pkg/trunk/robot_descriptions/gazebo_robot_description/world/pr2.model 2008-07-28 23:54:01 UTC (rev 2216)
+++ pkg/trunk/robot_descriptions/gazebo_robot_description/world/pr2.model 2008-07-29 00:05:38 UTC (rev 2217)
@@ -137,7 +137,7 @@
<!--
-->
<!--
- <sensor:ray2 name="laser_1">
+ <sensor:ray2 name="base_laser_1">
<rayCount>683</rayCount>
<rangeCount>683</rangeCount>
<laserCount>1</laserCount>
@@ -148,8 +148,8 @@
<minRange>0.05</minRange>
<maxRange>10.0</maxRange>
- <controller:sick2lms200_laser name="laser_controller_1">
- <interface:laser name="laser_iface_1"/>
+ <controller:sick2lms200_laser name="base_laser_controller_1">
+ <interface:laser name="base_laser_iface_1"/>
<updateRate>10</updateRate>
</controller:sick2lms200_laser>
</sensor:ray2>
@@ -1227,12 +1227,12 @@
<highStop> 0.0 </highStop>
</joint:hinge>
- <!-- Hokuyo laser body -->
- <body:box name="laser_body">
+ <!-- Hokuyo tilting laser body -->
+ <body:box name="tilt_laser_body">
<xyz>0.23 0.0 1.0</xyz>
<rpy>0.0 0.0 0.0</rpy>
- <geom:box name="laser_box">
+ <geom:box name="tilt_laser_box">
<xyz>0.0 0.0 0.02</xyz>
<rpy>0 0 0</rpy>
<size>0.05 0.05 0.041</size>
@@ -1245,7 +1245,7 @@
</visual>
</geom:box>
- <geom:cylinder name="laser_cylinder1">
+ <geom:cylinder name="tilt_laser_cylinder1">
<xyz>0.0 0.0 0.041</xyz>
<rpy>0 0 0</rpy>
<size>0.02 0.013</size>
@@ -1260,7 +1260,7 @@
</geom:cylinder>
- <geom:cylinder name="laser_cylinder2">
+ <geom:cylinder name="tilt_laser_cylinder2">
<xyz>0.0 0.0 0.054</xyz>
<rpy>0 0 0</rpy>
<size>0.018 0.009</size>
@@ -1275,7 +1275,7 @@
</geom:cylinder>
- <geom:cylinder name="laser_cylinder3">
+ <geom:cylinder name="tilt_laser_cylinder3">
<xyz>0.0 0.0 0.063</xyz>
<rpy>0 0 0</rpy>
<size>0.0175 0.008</size>
@@ -1291,8 +1291,8 @@
</geom:cylinder>
- <sensor:ray name="laser_1">
- <rayCount>68</rayCount>
+ <sensor:ray name="tilt_laser_1">
+ <rayCount>683</rayCount>
<rangeCount>683</rangeCount>
<laserCount>1</laserCount>
<origin>0.0 0.0 0.05</origin>
@@ -1303,15 +1303,15 @@
<minRange>0.05</minRange>
<maxRange>10.0</maxRange>
<updateRate>10</updateRate>
- <controller:sicklms200_laser name="laser_controller_1">
- <interface:laser name="laser_iface_1"/>
+ <controller:sicklms200_laser name="tilt_laser_controller_1">
+ <interface:laser name="tilt_laser_iface_1"/>
<updateRate>10</updateRate>
</controller:sicklms200_laser>
</sensor:ray>
<!--
-->
<!--
- <sensor:ray2 name="laser_1">
+ <sensor:ray2 name="tilt_laser_1">
<rayCount>683</rayCount>
<rangeCount>683</rangeCount>
<laserCount>1</laserCount>
@@ -1322,8 +1322,8 @@
<minRange>0.05</minRange>
<maxRange>10.0</maxRange>
- <controller:sick2lms200_laser name="laser_controller_1">
- <interface:laser name="laser_iface_1"/>
+ <controller:sick2lms200_laser name="tilt_laser_controller_1">
+ <interface:laser name="tilt_laser_iface_1"/>
<updateRate>10</updateRate>
</controller:sick2lms200_laser>
</sensor:ray2>
@@ -1334,8 +1334,8 @@
<!-- attach hokuyo to torso -->
<joint:hinge name="hokuyo_pitch_joint">
<body1>neck_body</body1>
- <body2>laser_body</body2>
- <anchor>laser_body</anchor>
+ <body2>tilt_laser_body</body2>
+ <anchor>tilt_laser_body</anchor>
<axis> 0.0 1.0 0.0 </axis>
</joint:hinge>
Modified: pkg/trunk/robot_descriptions/gazebo_robot_description/world/pr2_new_plugins.model
===================================================================
--- pkg/trunk/robot_descriptions/gazebo_robot_description/world/pr2_new_plugins.model 2008-07-28 23:54:01 UTC (rev 2216)
+++ pkg/trunk/robot_descriptions/gazebo_robot_description/world/pr2_new_plugins.model 2008-07-29 00:05:38 UTC (rev 2217)
@@ -1169,11 +1169,11 @@
</joint:hinge>
<!-- Hokuyo laser body -->
- <body:box name="laser_body">
+ <body:box name="tilt_laser_body">
<xyz>0.23 0.0 1.0</xyz>
<rpy>0.0 0.0 0.0</rpy>
- <geom:box name="laser_box">
+ <geom:box name="tilt_laser_box">
<xyz>0.0 0.0 0.02</xyz>
<rpy>0 0 0</rpy>
<size>0.05 0.05 0.041</size>
@@ -1186,7 +1186,7 @@
</visual>
</geom:box>
- <geom:cylinder name="laser_cylinder1">
+ <geom:cylinder name="tilt_laser_cylinder1">
<xyz>0.0 0.0 0.041</xyz>
<rpy>0 0 0</rpy>
<size>0.02 0.013</size>
@@ -1201,7 +1201,7 @@
</geom:cylinder>
- <geom:cylinder name="laser_cylinder2">
+ <geom:cylinder name="tilt_laser_cylinder2">
<xyz>0.0 0.0 0.054</xyz>
<rpy>0 0 0</rpy>
<size>0.018 0.009</size>
@@ -1216,7 +1216,7 @@
</geom:cylinder>
- <geom:cylinder name="laser_cylinder3">
+ <geom:cylinder name="tilt_laser_cylinder3">
<xyz>0.0 0.0 0.063</xyz>
<rpy>0 0 0</rpy>
<size>0.0175 0.008</size>
@@ -1232,7 +1232,7 @@
</geom:cylinder>
- <sensor:ray name="tilting_laser">
+ <sensor:ray name="tilt_laser">
<rayCount>68</rayCount>
<rangeCount>683</rangeCount>
<laserCount>1</laserCount>
@@ -1244,20 +1244,20 @@
<minRange>0.05</minRange>
<maxRange>10.0</maxRange>
<updateRate>10</updateRate>
- <controller:ros_laser name="ros_tilting_laser_controller" plugin="libRos_Laser.so">
+ <controller:ros_laser name="ros_tilt_laser_controller" plugin="libRos_Laser.so">
<updateRate>15.0</updateRate>
- <topicName>tilting_laser</topicName>
- <interface:laser name="ros_tilting_laser_iface" />
+ <topicName>tilt_laser</topicName>
+ <interface:laser name="ros_tilt_laser_iface" />
</controller:ros_laser>
- <controller:sicklms200_laser name="tilting_laser_controller">
- <interface:laser name="tiltiing_laser_iface"/>
+ <controller:sicklms200_laser name="tilt_laser_controller">
+ <interface:laser name="tilt_laser_iface"/>
<updateRate>10</updateRate>
</controller:sicklms200_laser>
</sensor:ray>
<!--
-->
<!--
- <sensor:ray2 name="tilting_laser">
+ <sensor:ray2 name="tilt_laser">
<rayCount>683</rayCount>
<rangeCount>683</rangeCount>
<laserCount>1</laserCount>
@@ -1268,7 +1268,7 @@
<minRange>0.05</minRange>
<maxRange>10.0</maxRange>
- <controller:sick2lms200_laser name="tilting_laser_controller">
+ <controller:sick2lms200_laser name="tilt_laser_controller">
<interface:laser name="tiltiing_laser_iface"/>
<updateRate>10</updateRate>
</controller:sick2lms200_laser>
@@ -1280,8 +1280,8 @@
<!-- attach hokuyo to torso -->
<joint:hinge name="hokuyo_pitch_joint">
<body1>neck_body</body1>
- <body2>laser_body</body2>
- <anchor>laser_body</anchor>
+ <body2>tilt_laser_body</body2>
+ <anchor>tilt_laser_body</anchor>
<axis> 0.0 1.0 0.0 </axis>
</joint:hinge>
Modified: pkg/trunk/robot_descriptions/gazebo_robot_description/world/pr2_torque.model
===================================================================
--- pkg/trunk/robot_descriptions/gazebo_robot_description/world/pr2_torque.model 2008-07-28 23:54:01 UTC (rev 2216)
+++ pkg/trunk/robot_descriptions/gazebo_robot_description/world/pr2_torque.model 2008-07-29 00:05:38 UTC (rev 2217)
@@ -138,7 +138,7 @@
<!--
-->
<!--
- <sensor:ray2 name="laser_1">
+ <sensor:ray2 name="base_laser_1">
<rayCount>683</rayCount>
<rangeCount>683</rangeCount>
<laserCount>1</laserCount>
@@ -149,8 +149,8 @@
<minRange>0.05</minRange>
<maxRange>10.0</maxRange>
- <controller:sick2lms200_laser name="laser_controller_1">
- <interface:laser name="laser_iface_1"/>
+ <controller:sick2lms200_laser name="base_laser_controller_1">
+ <interface:laser name="base_laser_iface_1"/>
<updateRate>10</updateRate>
</controller:sick2lms200_laser>
</sensor:ray2>
@@ -159,7 +159,7 @@
</body:box>
<!-- attach hokuyo to torso -->
- <joint:hinge name="tiltlaser_torso_attach_joint">
+ <joint:hinge name="base_laser_torso_attach_joint">
<body1>base_body</body1>
<body2>base_laser_body</body2>
<anchor>base_laser_body</anchor>
@@ -1125,11 +1125,11 @@
</joint:hinge>
<!-- Hokuyo laser body -->
- <body:box name="laser_body">
+ <body:box name="tilt_laser_body">
<xyz>0.23 0.0 1.0</xyz>
<rpy>0.0 0.0 0.0</rpy>
- <geom:box name="laser_box">
+ <geom:box name="tilt_laser_box">
<xyz>0.0 0.0 0.02</xyz>
<rpy>0 0 0</rpy>
<size>0.05 0.05 0.041</size>
@@ -1142,7 +1142,7 @@
</visual>
</geom:box>
- <geom:cylinder name="laser_cylinder1">
+ <geom:cylinder name="tilt_laser_cylinder1">
<xyz>0.0 0.0 0.041</xyz>
<rpy>0 0 0</rpy>
<size>0.02 0.013</size>
@@ -1157,7 +1157,7 @@
</geom:cylinder>
- <geom:cylinder name="laser_cylinder2">
+ <geom:cylinder name="tilt_laser_cylinder2">
<xyz>0.0 0.0 0.054</xyz>
<rpy>0 0 0</rpy>
<size>0.018 0.009</size>
@@ -1172,7 +1172,7 @@
</geom:cylinder>
- <geom:cylinder name="laser_cylinder3">
+ <geom:cylinder name="tilt_laser_cylinder3">
<xyz>0.0 0.0 0.063</xyz>
<rpy>0 0 0</rpy>
<size>0.0175 0.008</size>
@@ -1188,7 +1188,7 @@
</geom:cylinder>
- <sensor:ray name="laser_1">
+ <sensor:ray name="tilt_laser_1">
<rayCount>68</rayCount>
<rangeCount>683</rangeCount>
<laserCount>1</laserCount>
@@ -1200,15 +1200,15 @@
<minRange>0.05</minRange>
<maxRange>10.0</maxRange>
<updateRate>10</updateRate>
- <controller:sicklms200_laser name="laser_controller_1">
- <interface:laser name="laser_iface_1"/>
+ <controller:sicklms200_laser name="tilt_laser_controller_1">
+ <interface:laser name="tilt_laser_iface_1"/>
<updateRate>10</updateRate>
</controller:sicklms200_laser>
</sensor:ray>
<!--
-->
<!--
- <sensor:ray2 name="laser_1">
+ <sensor:ray2 name="tilt_laser_1">
<rayCount>683</rayCount>
<rangeCount>683</rangeCount>
<laserCount>1</laserCount>
@@ -1219,8 +1219,8 @@
<minRange>0.05</minRange>
<maxRange>10.0</maxRange>
- <controller:sick2lms200_laser name="laser_controller_1">
- <interface:laser name="laser_iface_1"/>
+ <controller:sick2lms200_laser name="tilt_laser_controller_1">
+ <interface:laser name="tilt_laser_iface_1"/>
<updateRate>10</updateRate>
</controller:sick2lms200_laser>
</sensor:ray2>
@@ -1231,8 +1231,8 @@
<!-- attach hokuyo to torso -->
<joint:hinge name="hokuyo_pitch_joint">
<body1>neck_body</body1>
- <body2>laser_body</body2>
- <anchor>laser_body</anchor>
+ <body2>tilt_laser_body</body2>
+ <anchor>tilt_laser_body</anchor>
<axis> 0.0 1.0 0.0 </axis>
</joint:hinge>
Modified: pkg/trunk/robot_descriptions/gazebo_robot_description/world/pr2_torque_control.model
===================================================================
--- pkg/trunk/robot_descriptions/gazebo_robot_description/world/pr2_torque_control.model 2008-07-28 23:54:01 UTC (rev 2216)
+++ pkg/trunk/robot_descriptions/gazebo_robot_description/world/pr2_torque_control.model 2008-07-29 00:05:38 UTC (rev 2217)
@@ -138,7 +138,7 @@
<!--
-->
<!--
- <sensor:ray2 name="laser_1">
+ <sensor:ray2 name="base_laser_1">
<rayCount>683</rayCount>
<rangeCount>683</rangeCount>
<laserCount>1</laserCount>
@@ -149,8 +149,8 @@
<minRange>0.05</minRange>
<maxRange>10.0</maxRange>
- <controller:sick2lms200_laser name="laser_controller_1">
- <interface:laser name="laser_iface_1"/>
+ <controller:sick2lms200_laser name="base_laser_controller_1">
+ <interface:laser name="base_laser_iface_1"/>
<updateRate>10</updateRate>
</controller:sick2lms200_laser>
</sensor:ray2>
@@ -159,7 +159,7 @@
</body:box>
<!-- attach hokuyo to torso -->
- <joint:hinge name="tiltlaser_torso_attach_joint">
+ <joint:hinge name="base_laser_torso_attach_joint">
<body1>base_body</body1>
<body2>base_laser_body</body2>
<anchor>base_laser_body</anchor>
@@ -1124,11 +1124,11 @@
</joint:hinge>
<!-- Hokuyo laser body -->
- <body:box name="laser_body">
+ <body:box name="tilt_laser_body">
<xyz>0.23 0.0 1.0</xyz>
<rpy>0.0 0.0 0.0</rpy>
- <geom:box name="laser_box">
+ <geom:box name="tilt_laser_box">
<xyz>0.0 0.0 0.02</xyz>
<rpy>0 0 0</rpy>
<size>0.05 0.05 0.041</size>
@@ -1141,7 +1141,7 @@
</visual>
</geom:box>
- <geom:cylinder name="laser_cylinder1">
+ <geom:cylinder name="tilt_laser_cylinder1">
<xyz>0.0 0.0 0.041</xyz>
<rpy>0 0 0</rpy>
<size>0.02 0.013</size>
@@ -1156,7 +1156,7 @@
</geom:cylinder>
- <geom:cylinder name="laser_cylinder2">
+ <geom:cylinder name="tilt_laser_cylinder2">
<xyz>0.0 0.0 0.054</xyz>
<rpy>0 0 0</rpy>
<size>0.018 0.009</size>
@@ -1171,7 +1171,7 @@
</geom:cylinder>
- <geom:cylinder name="laser_cylinder3">
+ <geom:cylinder name="tilt_laser_cylinder3">
<xyz>0.0 0.0 0.063</xyz>
<rpy>0 0 0</rpy>
<size>0.0175 0.008</size>
@@ -1187,7 +1187,7 @@
</geom:cylinder>
- <sensor:ray name="laser_1">
+ <sensor:ray name="tilt_laser_1">
<rayCount>68</rayCount>
<rangeCount>683</rangeCount>
<laserCount>1</laserCount>
@@ -1199,15 +1199,15 @@
<minRange>0.05</minRange>
<maxRange>10.0</maxRange>
<updateRate>10</updateRate>
- <controller:sicklms200_laser name="laser_controller_1">
- <interface:laser name="laser_iface_1"/>
+ <controller:sicklms200_laser name="tilt_laser_controller_1">
+ <interface:laser name="tilt_laser_iface_1"/>
<updateRate>10</updateRate>
</controller:sicklms200_laser>
</sensor:ray>
<!--
-->
<!--
- <sensor:ray2 name="laser_1">
+ <sensor:ray2 name="tilt_laser_1">
<rayCount>683</rayCount>
<rangeCount>683</rangeCount>
<laserCount>1</laserCount>
@@ -1218,8 +1218,8 @@
<minRange>0.05</minRange>
<maxRange>10.0</maxRange>
- <controller:sick2lms200_laser name="laser_controller_1">
- <interface:laser name="laser_iface_1"/>
+ <controller:sick2lms200_laser name="tilt_laser_controller_1">
+ <interface:laser name="tilt_laser_iface_1"/>
<updateRate>10</updateRate>
</controller:sick2lms200_laser>
</sensor:ray2>
@@ -1230,8 +1230,8 @@
<!-- attach hokuyo to torso -->
<joint:hinge name="hokuyo_pitch_joint">
<body1>neck_body</body1>
- <body2>laser_body</body2>
- <anchor>laser_body</anchor>
+ <body2>tilt_laser_body</body2>
+ <anchor>tilt_laser_body</anchor>
<axis> 0.0 1.0 0.0 </axis>
</joint:hinge>
Modified: pkg/trunk/robot_descriptions/gazebo_robot_description/world/pr2_torque_position.model
===================================================================
--- pkg/trunk/robot_descriptions/gazebo_robot_description/world/pr2_torque_position.model 2008-07-28 23:54:01 UTC (rev 2216)
+++ pkg/trunk/robot_descriptions/gazebo_robot_description/world/pr2_torque_position.model 2008-07-29 00:05:38 UTC (rev 2217)
@@ -138,7 +138,7 @@
<!--
-->
<!--
- <sensor:ray2 name="laser_1">
+ <sensor:ray2 name="base_laser_1">
<rayCount>683</rayCount>
<rangeCount>683</rangeCount>
<laserCount>1</laserCount>
@@ -149,8 +149,8 @@
<minRange>0.05</minRange>
<maxRange>10.0</maxRange>
- <controller:sick2lms200_laser name="laser_controller_1">
- <interface:laser name="laser_iface_1"/>
+ <controller:sick2lms200_laser name="base_laser_controller_1">
+ <interface:laser name="base_laser_iface_1"/>
<updateRate>10</updateRate>
</controller:sick2lms200_laser>
</sensor:ray2>
@@ -159,7 +159,7 @@
</body:box>
<!-- attach hokuyo to torso -->
- <joint:hinge name="tiltlaser_torso_attach_joint">
+ <joint:hinge name="base_laser_torso_attach_joint">
<body1>base_body</body1>
<body2>base_laser_body</body2>
<anchor>base_laser_body</anchor>
@@ -1123,11 +1123,11 @@
</joint:hinge>
<!-- Hokuyo laser body -->
- <body:box name="laser_body">
+ <body:box name="tilt_laser_body">
<xyz>0.23 0.0 1.0</xyz>
<rpy>0.0 0.0 0.0</rpy>
- <geom:box name="laser_box">
+ <geom:box name="tilt_laser_box">
<xyz>0.0 0.0 0.02</xyz>
<rpy>0 0 0</rpy>
<size>0.05 0.05 0.041</size>
@@ -1140,7 +1140,7 @@
</visual>
</geom:box>
- <geom:cylinder name="laser_cylinder1">
+ <geom:cylinder name="tilt_laser_cylinder1">
<xyz>0.0 0.0 0.041</xyz>
<rpy>0 0 0</rpy>
<size>0.02 0.013</size>
@@ -1155,7 +1155,7 @@
</geom:cylinder>
- <geom:cylinder name="laser_cylinder2">
+ <geom:cylinder name="tilt_laser_cylinder2">
<xyz>0.0 0.0 0.054</xyz>
<rpy>0 0 0</rpy>
<size>0.018 0.009</size>
@@ -1170,7 +1170,7 @@
</geom:cylinder>
- <geom:cylinder name="laser_cylinder3">
+ <geom:cylinder name="tilt_laser_cylinder3">
<xyz>0.0 0.0 0.063</xyz>
<rpy>0 0 0</rpy>
<size>0.0175 0.008</size>
@@ -1186,7 +1186,7 @@
</geom:cylinder>
- <sensor:ray name="laser_1">
+ <sensor:ray name="tilt_laser_1">
<rayCount>68</rayCount>
<rangeCount>683</rangeCount>
<laserCount>1</laserCount>
@@ -1198,15 +1198,15 @@
<minRange>0.05</minRange>
<maxRange>10.0</maxRange>
<updateRate>10</updateRate>
- <controller:sicklms200_laser name="laser_controller_1">
- <interface:laser name="laser_iface_1"/>
+ <controller:sicklms200_laser name="tilt_laser_controller_1">
+ <interface:laser name="tilt_laser_iface_1"/>
<updateRate>10</updateRate>
</controller:sicklms200_laser>
</sensor:ray>
<!--
-->
<!--
- <sensor:ray2 name="laser_1">
+ <sensor:ray2 name="tilt_laser_1">
<rayCount>683</rayCount>
<rangeCount>683</rangeCount>
<laserCount>1</laserCount>
@@ -1217,8 +1217,8 @@
<minRange>0.05</minRange>
<maxRange>10.0</maxRange>
- <controller:sick2lms200_laser name="laser_controller_1">
- <interface:laser name="laser_iface_1"/>
+ <controller:sick2lms200_laser name="tilt_laser_controller_1">
+ <interface:laser name="tilt_laser_iface_1"/>
<updateRate>10</updateRate>
</controller:sick2lms200_laser>
</sensor:ray2>
@@ -1229,8 +1229,8 @@
<!-- attach hokuyo to torso -->
<joint:hinge name="hokuyo_pitch_joint">
<body1>neck_body</body1>
- <body2>laser_body</body2>
- <anchor>laser_body</anchor>
+ <body2>tilt_laser_body</body2>
+ <anchor>tilt_laser_body</anchor>
<axis> 0.0 1.0 0.0 </axis>
</joint:hinge>
Modified: pkg/trunk/robot_descriptions/gazebo_robot_description/world/pr2_trimesh.model
===================================================================
--- pkg/trunk/robot_descriptions/gazebo_robot_description/world/pr2_trimesh.model 2008-07-28 23:54:01 UTC (rev 2216)
+++ pkg/trunk/robot_descriptions/gazebo_robot_description/world/pr2_trimesh.model 2008-07-29 00:05:38 UTC (rev 2217)
@@ -134,7 +134,7 @@
<!--
-->
<!--
- <sensor:ray2 name="laser_1">
+ <sensor:ray2 name="base_laser_1">
<rayCount>683</rayCount>
<rangeCount>683</rangeCount>
<laserCount>1</laserCount>
@@ -145,8 +145,8 @@
<minRange>0.05</minRange>
<maxRange>10.0</maxRange>
- <controller:sick2lms200_laser name="laser_controller_1">
- <interface:laser name="laser_iface_1"/>
+ <controller:sick2lms200_laser name="base_laser_controller_1">
+ <interface:laser name="base_laser_iface_1"/>
<updateRate>10</updateRate>
</controller:sick2lms200_laser>
</sensor:ray2>
@@ -154,7 +154,7 @@
</body:box>
<!-- attach hokuyo to torso -->
- <joint:hinge name="tiltlaser_torso_attach_joint">
+ <joint:hinge name="base_laser_torso_attach_joint">
<body1>base_body</body1>
<body2>base_laser_body</body2>
<anchor>base_laser_body</anchor>
@@ -1123,11 +1123,11 @@
</joint:hinge>
<!-- Hokuyo laser body -->
- <body:box name="laser_body">
+ <body:box name="tilt_laser_body">
<xyz>0.23 0.0 1.0</xyz>
<rpy>0.0 0.0 0.0</rpy>
- <geom:box name="laser_box">
+ <geom:box name="tilt_laser_box">
<xyz>0.0 0.0 0.02</xyz>
<rpy>0 0 0</rpy>
<size>0.05 0.05 0.041</size>
@@ -1140,7 +1140,7 @@
</visual>
</geom:box>
- <geom:cylinder name="laser_cylinder1">
+ <geom:cylinder name="tilt_laser_cylinder1">
<xyz>0.0 0.0 0.041</xyz>
<rpy>0 0 0</rpy>
<size>0.02 0.013</size>
@@ -1155,7 +1155,7 @@
</geom:cylinder>
- <geom:cylinder name="laser_cylinder2">
+ <geom:cylinder name="tilt_laser_cylinder2">
<xyz>0.0 0.0 0.054</xyz>
<rpy>0 0 0</rpy>
<size>0.018 0.009</size>
@@ -1170,7 +1170,7 @@
</geom:cylinder>
- <geom:cylinder name="laser_cylinder3">
+ <geom:cylinder name="tilt_laser_cylinder3">
<xyz>0.0 0.0 0.063</xyz>
<rpy>0 0 0</rpy>
<size>0.0175 0.008</size>
@@ -1186,7 +1186,7 @@
</geom:cylinder>
- <sensor:ray name="laser_1">
+ <sensor:ray name="tilt_laser_1">
<rayCount>68</rayCount>
<rangeCount>683</rangeCount>
<laserCount>1</laserCount>
@@ -1198,15 +1198,15 @@
<minRange>0.05</minRange>
<maxRange>10.0</maxRange>
<updateRate>10</updateRate>
- <controller:sicklms200_laser name="laser_controller_1">
- <interface:laser name="laser_iface_1"/>
+ <controller:sicklms200_laser name="tilt_laser_controller_1">
+ <interface:laser name="tilt_laser_iface_1"/>
<updateRate>10</updateRate>
</controller:sicklms200_laser>
</sensor:ray>
<!--
-->
<!--
- <sensor:ray2 name="laser_1">
+ <sensor:ray2 name="tilt_laser_1">
<rayCount>683</rayCount>
<rangeCount>683</rangeCount>
<laserCount>1</laserCount>
@@ -1217,8 +1217,8 @@
<minRange>0.05</minRange>
<maxRange>10.0</maxRange>
- <controller:sick2lms200_laser name="laser_controller_1">
- <interface:laser name="laser_iface_1"/>
+ <controller:sick2lms200_laser name="tilt_laser_controller_1">
+ <interface:laser name="tilt_laser_iface_1"/>
<updateRate>10</updateRate>
</controller:sick2lms200_laser>
</sensor:ray2>
@@ -1229,8 +1229,8 @@
<!-- attach hokuyo to torso -->
<joint:hinge name="hokuyo_pitch_joint">
<body1>neck_body</body1>
- <body2>laser_body</body2>
- <anchor>laser_body</anchor>
+ <body2>tilt_laser_body</body2>
+ <anchor>tilt_laser_body</anchor>
<axis> 0.0 1.0 0.0 </axis>
</joint:hinge>
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <rob...@us...> - 2008-07-29 02:04:41
|
Revision: 2229
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=2229&view=rev
Author: rob_wheeler
Date: 2008-07-29 02:04:47 +0000 (Tue, 29 Jul 2008)
Log Message:
-----------
Checkpoint an example of using the controllers over EtherCAT.
Modified Paths:
--------------
pkg/trunk/controllers/genericControllers/include/genericControllers/JointController.h
pkg/trunk/controllers/genericControllers/src/JointController.cpp
pkg/trunk/drivers/motor/ethercat_hardware/include/ethercat_hardware/ethercat_hardware.h
pkg/trunk/drivers/motor/ethercat_hardware/include/ethercat_hardware/mk1000.h
pkg/trunk/drivers/motor/ethercat_hardware/include/ethercat_hardware/mk1001.h
pkg/trunk/drivers/motor/ethercat_hardware/include/ethercat_hardware/motor_control_board.h
pkg/trunk/drivers/motor/ethercat_hardware/include/ethercat_hardware/wg05.h
pkg/trunk/drivers/motor/ethercat_hardware/src/ethercat_hardware.cpp
pkg/trunk/drivers/motor/ethercat_hardware/src/mk1001.cpp
pkg/trunk/drivers/motor/ethercat_hardware/src/wg05.cpp
pkg/trunk/mechanism/mechanism_control/CMakeLists.txt
pkg/trunk/mechanism/mechanism_control/manifest.xml
pkg/trunk/robot_control_loops/pr2_etherCAT/src/main.cpp
Added Paths:
-----------
pkg/trunk/mechanism/mechanism_control/include/mechanism_control/single_control.h
pkg/trunk/mechanism/mechanism_control/src/single_control.cpp
Modified: pkg/trunk/controllers/genericControllers/include/genericControllers/JointController.h
===================================================================
--- pkg/trunk/controllers/genericControllers/include/genericControllers/JointController.h 2008-07-29 01:35:41 UTC (rev 2228)
+++ pkg/trunk/controllers/genericControllers/include/genericControllers/JointController.h 2008-07-29 02:04:47 UTC (rev 2229)
@@ -36,7 +36,7 @@
/***************************************************/
/*! \class controller::JointController
\brief A Joint controller
-
+
This class implements controller loops for
PR2 Joint Control
@@ -62,7 +62,7 @@
4. Set appropriate command (position, torque, velocity)
5. Call Update from Real Time loop
-
+
*/
/***************************************************/
#include <sys/types.h>
@@ -88,7 +88,7 @@
class JointController : Controller
{
- static const double AccelerationThreshold = 0.1; //Distance threshold below which linear acceleration limit is active, if enabled
+ static const double AccelerationThreshold = 0.1; //Distance threshold below which linear acceleration limit is active, if enabled
public:
/*!
@@ -96,10 +96,10 @@
*
*/
JointController();
-
+
/*!
* \brief Destructor of the JointController class.
- */
+ */
~JointController( );
static Controller* create(const std::string&) {
@@ -111,16 +111,16 @@
* \param Joint* joint The joint we are interacting with
* \param string name The namespace identification in ROS
*/
- // controller::controllerErrorCode Init(Joint* joint, string name);
+ // controller::controllerErrorCode Init(Joint* joint, string name);
// JointController(Joint* joint, string name);
- /*!
+ /*!
* \brief Functional way to initialize limits and gains.
*
*/
void init(pidControlParam pcp, controllerControlMode mode, double time, double maxEffort, double minEffort, mechanism::Joint *joint);
- /*!
+ /*!
* \brief Functional way to initialize limits and gains.
*
*/
@@ -132,10 +132,10 @@
* \param string name The namespace identification in ROS
*/
// controller::controllerErrorCode Init(mechanism::Joint* joint, string name);
-
+
// JointController(Joint* joint, string name);
- /*!
+ /*!
* \brief Functional way to initialize limits and gains.
*
*/
@@ -143,19 +143,19 @@
/*!
* \brief TODO: Get the actual time
- *
*
- * \param double* time Pointer to value to change
+ *
+ * \param double* time Pointer to value to change
*/
void getTime(double* time);
-
+
//---------------------------------------------------------------------------------//
//MODE/ENABLE CALLS
//---------------------------------------------------------------------------------//
-
+
/*!
* \brief Switches command mode type (Torque, position, velocity control)
- *
+ *
*/
controllerControlMode setMode(controller::controllerControlMode mode);
@@ -176,9 +176,9 @@
controller::controllerControlMode disableController();
/*!
- * \brief Return true if last command saturated the torque
+ * \brief Return true if last command saturated the torque
*
- *
+ *
*/
bool checkForSaturation(void);
@@ -191,19 +191,19 @@
* \param torque Torque command to issue
*/
controller::controllerErrorCode setTorqueCmd(double torque);
-
+
/*!
- * \brief Fetch the latest user issued torque command
- *
- * \param double* torque Pointer to value to change
- */
+ * \brief Fetch the latest user issued torque command
+ *
+ * \param double* torque Pointer to value to change
+ */
controller::controllerErrorCode getTorqueCmd(double *torque);
-
+
/*!
* \brief Get the actual torque of the joint motor.
- *
+ *
* \param double* torque Pointer to value to change
- */
+ */
controller::controllerErrorCode getTorqueAct(double *torque);
//---------------------------------------------------------------------------------//
@@ -212,45 +212,45 @@
/*!
* \brief Give set position of the joint for next update: revolute (angle) and prismatic (position)
- *
+ *
* \param double pos Position command to issue
- */
+ */
controller::controllerErrorCode setPosCmd(double pos);
-
+
/*!
* \brief Get latest position command to the joint: revolute (angle) and prismatic (position).
* \param double* pos Pointer to value to change
- */
+ */
controller::controllerErrorCode getPosCmd(double *pos);
-
+
/*!
* \brief Read the torque of the motor
* \param double* pos Pointer to value to change
- */
- controller::controllerErrorCode getPosAct(double *pos);
+ */
+ controller::controllerErrorCode getPosAct(double *pos);
//---------------------------------------------------------------------------------//
//VELOCITY CALLS
//---------------------------------------------------------------------------------//
-
+
/*!
* \brief Set velocity command to the joint to be issue next update
* \param double vel Velocity to issue next command
*/
controller::controllerErrorCode setVelCmd(double vel);
-
+
/*!
* \brief Get latest velocity command to the joint
* \param double* vel Pointer to value to change
*/
controller::controllerErrorCode getVelCmd(double *vel);
-
+
/*!
* \brief Get actual velocity of the joint
* \param double* vel Pointer to value to change
*/
controller::controllerErrorCode getVelAct(double *vel);
-
+
/*!
* \brief Compute max velocity coming into the end stop to stop with linear velocity in endstop.
*
@@ -263,12 +263,14 @@
/*!
* \brief Issues commands to joint based on control mode
*
- *
+ *
*/
//Issues commands to the joint. Should be called at regular intervals
virtual void update(void);
+ virtual void update(double current_time);
+
//---------------------------------------------------------------------------------//
//PARAM SERVER CALLS
//---------------------------------------------------------------------------------//
@@ -280,7 +282,7 @@
* constraints for this controller
* \param string label Name of param to change
* \param double value New value
- *<br>
+ *<br>
*<UL TYPE="none">
*<LI> e.g. SetParam('PGain',10);
*<LI> or SetParam('IGain',10);
@@ -298,7 +300,7 @@
* user can get maximum velocity
* and maximum acceleration
* constraints for this controller
- *<br>
+ *<br>
*<UL TYPE="none">
*<UL TYPE="none">
*<LI> e.g. GetParam('PGain',value);
@@ -310,9 +312,9 @@
*/
controller::controllerErrorCode getParam(std::string label, double* value);
//controller::controllerErrorCode GetParam(std::string label, std::string value);
-
+
/*!
- * \brief Set the name of the controller
+ * \brief Set the name of the controller
*
* \param name - std::string representation of the name of the controller
*/
@@ -341,13 +343,13 @@
/*!
* \brief Actually issue torque set command of the joint motor.
- */
+ */
void setJointEffort(double torque);
-
- mechanism::Joint* joint; /*!< Joint we're controlling>*/
- Pid pidController; /*!< Internal PID controller>*/
+ mechanism::Joint* joint; /*!< Joint we're controlling>*/
+ Pid pidController; /*!< Internal PID controller>*/
+
double lastTime;/*!< Last time stamp of update> */
double cmdTorque;/*!< Last commanded torque>*/
@@ -356,11 +358,11 @@
double cmdVel;/*!< Last commanded Velocity> */
- bool saturationFlag; /*!< Flag to indicate last command exceed torque limits and was truncated>*/
+ bool saturationFlag; /*!< Flag to indicate last command exceed torque limits and was truncated>*/
bool enabled; /*!<Can controller issue commands?>*/
- controller::controllerControlMode controlMode; /*!< Indicate current controller mode (torque, position, velocity)>*/
+ controller::controllerControlMode controlMode; /*!< Indicate current controller mode (torque, position, velocity)>*/
double pGain; /*!< Proportional gain>*/
@@ -382,7 +384,7 @@
void loadParam(std::string label, int &value);
- std::string name; /*!< Namespace ID for this controller>*/
+ std::string name; /*!< Namespace ID for this controller>*/
};
}
Modified: pkg/trunk/controllers/genericControllers/src/JointController.cpp
===================================================================
--- pkg/trunk/controllers/genericControllers/src/JointController.cpp 2008-07-29 01:35:41 UTC (rev 2228)
+++ pkg/trunk/controllers/genericControllers/src/JointController.cpp 2008-07-29 02:04:47 UTC (rev 2229)
@@ -37,45 +37,45 @@
#include <sys/time.h>
#define DEFAULTMAXACCEL 5
//#define DEBUG 1
-//Todo:
+//Todo:
//1. Get and set params via server
//2. Integrate Joint and robot objects
//3. Integrate Motor controller time
using namespace controller;
-
+
//---------------------------------------------------------------------------------//
//CONSTRUCTION/DESTRUCTION CALLS
//---------------------------------------------------------------------------------//
JointController::JointController()
-{
+{
//Instantiate PID class
- pidController.InitPid(0,0,0,0,0); //Constructor for pid controller
+ pidController.InitPid(0,0,0,0,0); //Constructor for pid controller
//Set commands to zero
cmdTorque = 0;
cmdPos = 0;
cmdVel = 0;
-
+
controlMode = controller::CONTROLLER_DISABLED;
}
JointController::~JointController( )
{
-
+
}
void JointController::init(double pGain, double iGain, double dGain, double windupMax, double windupMin, controllerControlMode mode, double time, double maxEffort, double minEffort, mechanism::Joint *joint) {
- pidController.InitPid(pGain,iGain,dGain,windupMax,windupMin); //Constructor for pid controller
+ pidController.InitPid(pGain,iGain,dGain,windupMax,windupMin); //Constructor for pid controller
cmdTorque = 0;
cmdPos = 0;
cmdVel = 0;
-
+
//Init time
lastTime = time;
@@ -87,7 +87,7 @@
this->windupMin = windupMin;
this->minEffort = minEffort;
- this->maxEffort = maxEffort;
+ this->maxEffort = maxEffort;
this->joint = joint;
controlMode = mode;
@@ -96,12 +96,12 @@
void JointController::init(pidControlParam pcp, controllerControlMode mode, double time, double maxEffort, double minEffort, Joint *joint) {
- pidController.InitPid(pcp.pGain,pcp.iGain,pcp.dGain,pcp.windupMax,pcp.windupMin); //Constructor for pid controller
+ pidController.InitPid(pcp.pGain,pcp.iGain,pcp.dGain,pcp.windupMax,pcp.windupMin); //Constructor for pid controller
cmdTorque = 0;
cmdPos = 0;
cmdVel = 0;
-
+
//Init time
lastTime = time;
@@ -113,7 +113,7 @@
this->windupMin = pcp.windupMin;
this->minEffort = minEffort;
- this->maxEffort = maxEffort;
+ this->maxEffort = maxEffort;
this->joint = joint;
@@ -123,12 +123,12 @@
void JointController::init(double time, Joint *joint) {
- pidController.InitPid(pGain,iGain,dGain,windupMax,windupMin); //Constructor for pid controller
+ pidController.InitPid(pGain,iGain,dGain,windupMax,windupMin); //Constructor for pid controller
cmdTorque = 0;
cmdPos = 0;
cmdVel = 0;
-
+
lastTime = time;
this->joint = joint;
enableController();
@@ -176,7 +176,7 @@
return CONTROLLER_DISABLED;
}
-bool JointController::checkForSaturation(void){
+bool JointController::checkForSaturation(void){
return saturationFlag;
}
@@ -187,12 +187,12 @@
//---------------------------------------------------------------------------------//
controllerErrorCode JointController::setTorqueCmd(double torque){
// double maxEffort = joint->effortLimit;
-
+
if(controlMode != CONTROLLER_TORQUE) //Make sure we're in torque command mode
return CONTROLLER_MODE_ERROR;
-
- cmdTorque = torque;
-
+
+ cmdTorque = torque;
+
if(cmdTorque >= maxEffort){ //Truncate to positive limit
cmdTorque = maxEffort;
return CONTROLLER_TORQUE_LIMIT;
@@ -201,7 +201,7 @@
cmdTorque = minEffort;
return CONTROLLER_TORQUE_LIMIT;
}
-
+
return CONTROLLER_CMD_SET;
}
@@ -214,7 +214,7 @@
controllerErrorCode JointController::getTorqueAct(double *torque)
{
*torque = joint->appliedEffort; //Read torque from joint
- return CONTROLLER_ALL_OK;
+ return CONTROLLER_ALL_OK;
}
//---------------------------------------------------------------------------------//
@@ -222,13 +222,13 @@
//---------------------------------------------------------------------------------//
-//Query mode, then set desired position
+//Query mode, then set desired position
controllerErrorCode JointController::setPosCmd(double pos)
{
if(controlMode != CONTROLLER_POSITION) //Make sure we're in position command mode
return CONTROLLER_MODE_ERROR;
-
- cmdPos = pos;
+
+ cmdPos = pos;
if(cmdPos >= joint->jointLimitMax && joint->type != mechanism::JOINT_CONTINUOUS){ //Truncate to positive limit
cmdPos = joint->jointLimitMax;
return CONTROLLER_JOINT_LIMIT;
@@ -237,21 +237,21 @@
cmdPos = joint->jointLimitMin;
return CONTROLLER_JOINT_LIMIT;
}
- return CONTROLLER_CMD_SET;
+ return CONTROLLER_CMD_SET;
}
//Return the current position command
controller::controllerErrorCode JointController::getPosCmd(double *pos)
{
*pos = cmdPos;
- return controller::CONTROLLER_ALL_OK;
+ return controller::CONTROLLER_ALL_OK;
}
//Query the joint for the actual position
controller::controllerErrorCode JointController::getPosAct(double *pos)
{
*pos = joint->position;
- return controller::CONTROLLER_ALL_OK;
+ return controller::CONTROLLER_ALL_OK;
}
//---------------------------------------------------------------------------------//
@@ -261,7 +261,7 @@
controllerErrorCode JointController::setVelCmd(double vel)
{
if(controlMode == CONTROLLER_VELOCITY || controlMode == ETHERDRIVE_SPEED){ //Make sure we're in velocity command mode
- cmdVel = vel;
+ cmdVel = vel;
return CONTROLLER_CMD_SET;
}
else return CONTROLLER_MODE_ERROR;
@@ -271,14 +271,14 @@
controller::controllerErrorCode JointController::getVelCmd(double *vel)
{
*vel = cmdVel;
- return controller::CONTROLLER_ALL_OK;
+ return controller::CONTROLLER_ALL_OK;
}
//Query our joint for velocity
controller::controllerErrorCode JointController::getVelAct(double *vel)
{
*vel = joint->velocity;
- return controller::CONTROLLER_ALL_OK;
+ return controller::CONTROLLER_ALL_OK;
}
@@ -300,15 +300,14 @@
{
double error(0),time(0),currentTorqueCmd(0);
double maxVelocity = cmdVel;
- double currentVoltageCmd,v_backemf,v_clamp_min,v_clamp_max,k;
+ double currentVoltageCmd,v_backemf,v_clamp_min,v_clamp_max,k;
-
+
if(controlMode == controller::CONTROLLER_DISABLED){
printf("JointController.cpp: Error:: controller disabled\n");
return; //If we're not initialized, don't try to interact
}
getTime(&time); //TODO: Replace time with joint->timeStep
-
switch(controlMode)
{
case CONTROLLER_TORQUE: //Pass through torque command
@@ -316,8 +315,8 @@
break;
case CONTROLLER_POSITION: //Close the loop around position
- if(joint->type == mechanism::JOINT_ROTARY || joint->type == mechanism::JOINT_CONTINUOUS)
- error = shortest_angular_distance(cmdPos, joint->position);
+ if(joint->type == mechanism::JOINT_ROTARY || joint->type == mechanism::JOINT_CONTINUOUS)
+ error = shortest_angular_distance(cmdPos, joint->position);
else
error = joint->position - cmdPos;
currentTorqueCmd = pidController.UpdatePid(error,time-lastTime);
@@ -332,16 +331,17 @@
}
}
error = joint->velocity - cmdVel;
+
currentTorqueCmd = pidController.UpdatePid(error,time-lastTime);
break;
case ETHERDRIVE_SPEED: // Use hack to contol speed in voltage control mode for the etherdrive
#ifdef DEBUG
printf("JC:: %f\n",cmdVel);
#endif
- currentVoltageCmd = cmdVel*20*60/(136*2*M_PI);
+ currentVoltageCmd = cmdVel*20*60/(136*2*M_PI);
v_backemf = joint->velocity*20*60/(136*2*M_PI);
- v_clamp_min = v_backemf - 3;// 0.655*16.7;
+ v_clamp_min = v_backemf - 3;// 0.655*16.7;
v_clamp_max = v_backemf + 3;//0.655*16.7;
k = 1.0/ 36.0;
@@ -358,16 +358,83 @@
default:
printf("JointController.cpp: Error:: invalid controlMode\n");
- currentTorqueCmd = 0;
+ currentTorqueCmd = 0;
}
lastTime = time;
- setJointEffort(currentTorqueCmd);
+ setJointEffort(currentTorqueCmd);
}
+void JointController::update(double time)
+{
+ double error(0),currentTorqueCmd(0);
+ double maxVelocity = cmdVel;
+ double currentVoltageCmd,v_backemf,v_clamp_min,v_clamp_max,k;
+ if(controlMode == controller::CONTROLLER_DISABLED){
+ printf("JointController.cpp: Error:: controller disabled\n");
+ return; //If we're not initialized, don't try to interact
+ }
+
+ switch(controlMode)
+ {
+ case CONTROLLER_TORQUE: //Pass through torque command
+ currentTorqueCmd = cmdTorque;
+ break;
+
+ case CONTROLLER_POSITION: //Close the loop around position
+ if(joint->type == mechanism::JOINT_ROTARY || joint->type == mechanism::JOINT_CONTINUOUS)
+ error = shortest_angular_distance(cmdPos, joint->position);
+ else
+ error = joint->position - cmdPos;
+ currentTorqueCmd = pidController.UpdatePid(error,time-lastTime);
+ break;
+
+ case CONTROLLER_VELOCITY: //Close the loop around velocity
+ if(capAccel){
+ maxVelocity = getMaxVelocity(); //Check max velocity coming into wall
+ if(fabs(cmdVel)>maxVelocity){
+ cmdVel = -maxVelocity; //Truncate velocity smoothly
+ // std::cout<<"*******************"<<cmdVel<<std::endl;
+ }
+ }
+ error = joint->velocity - cmdVel;
+ currentTorqueCmd = pidController.UpdatePid(error,time-lastTime);
+ break;
+ case ETHERDRIVE_SPEED: // Use hack to contol speed in voltage control mode for the etherdrive
+#ifdef DEBUG
+ printf("JC:: %f\n",cmdVel);
+#endif
+ currentVoltageCmd = cmdVel*20*60/(136*2*M_PI);
+ v_backemf = joint->velocity*20*60/(136*2*M_PI);
+
+ v_clamp_min = v_backemf - 3;// 0.655*16.7;
+ v_clamp_max = v_backemf + 3;//0.655*16.7;
+
+ k = 1.0/ 36.0;
+#ifdef DEBUG
+ printf("JC::%f\t%f\t%f\n", v_clamp_min, currentVoltageCmd, v_clamp_max);
+#endif
+ if (currentVoltageCmd > v_clamp_max)
+ currentVoltageCmd = v_clamp_max;
+
+ if (currentVoltageCmd < v_clamp_min)
+ currentVoltageCmd = v_clamp_min;
+ currentTorqueCmd = currentVoltageCmd * k; //Convert to match PWM conversion inside boards
+ break;
+
+ default:
+ printf("JointController.cpp: Error:: invalid controlMode\n");
+ currentTorqueCmd = 0;
+ }
+ lastTime = time;
+
+ setJointEffort(currentTorqueCmd);
+}
+
+
controllerErrorCode JointController::setParam(std::string label,double value)
-{
+{
return CONTROLLER_ALL_OK;
}
@@ -381,7 +448,7 @@
void JointController::setJointEffort(double effort)
{
double newEffort;
-
+
newEffort = effort;
saturationFlag = false;
@@ -393,7 +460,7 @@
newEffort = minEffort;
saturationFlag = true;
}
- joint->commandedEffort = newEffort;
+ joint->commandedEffort = newEffort;
}
controllerErrorCode JointController::loadXML(std::string filename)
@@ -435,7 +502,7 @@
if(!exists)
return CONTROLLER_MODE_ERROR;
- paramMap = data.getDataTagValues("controller",this->name);
+ paramMap = data.getDataTagValues("controller",this->name);
loadParam("pGain",pGain);
loadParam("dGain",dGain);
Modified: pkg/trunk/drivers/motor/ethercat_hardware/include/ethercat_hardware/ethercat_hardware.h
===================================================================
--- pkg/trunk/drivers/motor/ethercat_hardware/include/ethercat_hardware/ethercat_hardware.h 2008-07-29 01:35:41 UTC (rev 2228)
+++ pkg/trunk/drivers/motor/ethercat_hardware/include/ethercat_hardware/ethercat_hardware.h 2008-07-29 02:04:47 UTC (rev 2229)
@@ -76,10 +76,12 @@
MotorControlBoard *configSlave(EtherCAT_SlaveHandler *sh);
MotorControlBoard **slaves;
- unsigned int numSlaves;
+ unsigned int num_slaves_;
- unsigned char *buffer;
- unsigned int bufferSize;
+ unsigned char *current_buffer_;
+ unsigned char *last_buffer_;
+ unsigned char *buffers_;
+ unsigned int buffer_size_;
};
#endif /* ETHERCAT_HARDWARE_H */
Modified: pkg/trunk/drivers/motor/ethercat_hardware/include/ethercat_hardware/mk1000.h
===================================================================
--- pkg/trunk/drivers/motor/ethercat_hardware/include/ethercat_hardware/mk1000.h 2008-07-29 01:35:41 UTC (rev 2228)
+++ pkg/trunk/drivers/motor/ethercat_hardware/include/ethercat_hardware/mk1000.h 2008-07-29 02:04:47 UTC (rev 2229)
@@ -48,9 +48,13 @@
void convertCommand(ActuatorCommand &command, unsigned char *buffer)
{
}
- void convertState(ActuatorState &state, unsigned char *buffer)
+ void convertState(ActuatorState &state, unsigned char *current_buffer, unsigned char *last_buffer)
{
}
+ bool hasActuator(void)
+ {
+ return false;
+ }
private:
static const EC_UDINT MK1000_PRODUCT_CODE = 0x000003E8;
};
Modified: pkg/trunk/drivers/motor/ethercat_hardware/include/ethercat_hardware/mk1001.h
===================================================================
--- pkg/trunk/drivers/motor/ethercat_hardware/include/ethercat_hardware/mk1001.h 2008-07-29 01:35:41 UTC (rev 2228)
+++ pkg/trunk/drivers/motor/ethercat_hardware/include/ethercat_hardware/mk1001.h 2008-07-29 02:04:47 UTC (rev 2229)
@@ -82,7 +82,11 @@
}
void configure(int &startAddress, EtherCAT_SlaveHandler *sh);
void convertCommand(ActuatorCommand &command, unsigned char *buffer);
- void convertState(ActuatorState &state, unsigned char *buffer);
+ void convertState(ActuatorState &state, unsigned char *current_buffer, unsigned char *last_buffer);
+ bool hasActuator(void)
+ {
+ return true;
+ }
private:
static const EC_UDINT MK1001_PRODUCT_CODE = 0x000003E9;
Modified: pkg/trunk/drivers/motor/ethercat_hardware/include/ethercat_hardware/motor_control_board.h
===================================================================
--- pkg/trunk/drivers/motor/ethercat_hardware/include/ethercat_hardware/motor_control_board.h 2008-07-29 01:35:41 UTC (rev 2228)
+++ pkg/trunk/drivers/motor/ethercat_hardware/include/ethercat_hardware/motor_control_board.h 2008-07-29 02:04:47 UTC (rev 2229)
@@ -55,7 +55,8 @@
}
virtual void configure(int &startAddress, EtherCAT_SlaveHandler *sh) = 0;
virtual void convertCommand(ActuatorCommand &command, unsigned char *buffer) = 0;
- virtual void convertState(ActuatorState &state, unsigned char *buffer) = 0;
+ virtual void convertState(ActuatorState &state, unsigned char *current_buffer, unsigned char *last_buffer) = 0;
+ virtual bool hasActuator(void) = 0;
EC_UDINT productCode;
unsigned int commandSize;
Modified: pkg/trunk/drivers/motor/ethercat_hardware/include/ethercat_hardware/wg05.h
===================================================================
--- pkg/trunk/drivers/motor/ethercat_hardware/include/ethercat_hardware/wg05.h 2008-07-29 01:35:41 UTC (rev 2228)
+++ pkg/trunk/drivers/motor/ethercat_hardware/include/ethercat_hardware/wg05.h 2008-07-29 02:04:47 UTC (rev 2229)
@@ -94,7 +94,11 @@
}
void configure(int &startAddress, EtherCAT_SlaveHandler *sh);
void convertCommand(ActuatorCommand &command, unsigned char *buffer);
- void convertState(ActuatorState &state, unsigned char *buffer);
+ void convertState(ActuatorState &state, unsigned char *current_buffer, unsigned char *last_buffer);
+ bool hasActuator(void)
+ {
+ return true;
+ }
private:
static const EC_UDINT WG05_PRODUCT_CODE = 0x57473035;
Modified: pkg/trunk/drivers/motor/ethercat_hardware/src/ethercat_hardware.cpp
===================================================================
--- pkg/trunk/drivers/motor/ethercat_hardware/src/ethercat_hardware.cpp 2008-07-29 01:35:41 UTC (rev 2228)
+++ pkg/trunk/drivers/motor/ethercat_hardware/src/ethercat_hardware.cpp 2008-07-29 02:04:47 UTC (rev 2229)
@@ -39,7 +39,7 @@
#include <dll/ethercat_dll.h>
EthercatHardware::EthercatHardware() :
- hw(0), ni(0), buffer(0), bufferSize(0)
+ hw(0), ni(0), current_buffer_(0), last_buffer_(0), buffer_size_(0)
{
}
@@ -49,10 +49,14 @@
{
close_socket(ni);
}
- if (buffer)
+ if (current_buffer_)
{
- delete buffer;
+ delete current_buffer_;
}
+ if (last_buffer_)
+ {
+ delete last_buffer_;
+ }
if (hw)
{
delete hw;
@@ -61,23 +65,6 @@
void EthercatHardware::init(char *interface, char *configuration)
{
- // Determine configuration from XML file 'configuration'
- // Create HardwareInterface
-
- // XXXwheeler: replace with XML parser
- hw = new HardwareInterface(5);
-
- hw->actuator[0].command.enable = true;
- hw->actuator[0].command.current = 0;
- hw->actuator[1].command.enable = true;
- hw->actuator[1].command.current = 0.0;
- hw->actuator[2].command.enable = true;
- hw->actuator[2].command.current = 0.0;
- hw->actuator[3].command.enable = true;
- hw->actuator[3].command.current = 0;
- hw->actuator[4].command.enable = true;
- hw->actuator[4].command.current = 0;
-
// Initialize network interface
if ((ni = init_ec(interface)) == NULL)
{
@@ -109,6 +96,7 @@
slaves = new MotorControlBoard*[numSlaves];
+ unsigned int num_actuators = 0;
for (unsigned int slave = 0; slave < numSlaves; ++slave)
{
EC_FixedStationAddress fsa(slave + 1);
@@ -121,39 +109,51 @@
if ((slaves[slave] = configSlave(sh)) != NULL)
{
- bufferSize += slaves[slave]->commandSize + slaves[slave]->statusSize;
+ num_actuators += slaves[slave]->hasActuator();
+ buffer_size_ += slaves[slave]->commandSize + slaves[slave]->statusSize;
if (!sh->to_state(EC_OP_STATE))
{
perror("to_state");
}
}
}
- buffer = new unsigned char[bufferSize];
+ buffers_ = new unsigned char[2 * buffer_size_];
+ current_buffer_ = buffers_;
+ last_buffer_ = buffers_ + buffer_size_;
+
+ // Determine configuration from XML file 'configuration'
+ // Create HardwareInterface
+ hw = new HardwareInterface(num_actuators);
}
void EthercatHardware::update()
{
- unsigned char *p;
+ unsigned char *current, *last;
// Convert HW Interface commands to MCB-specific buffers
- p = buffer;
+ current = current_buffer_;
for (int i = 0; i < hw->numActuators; ++i)
{
- slaves[i]->convertCommand(hw->actuator[i].command, p);
- p += slaves[i]->commandSize + slaves[i]->statusSize;
+ slaves[i]->convertCommand(hw->actuator[i].command, current);
+ current += slaves[i]->commandSize + slaves[i]->statusSize;
}
// Transmit process data
- em->txandrx_PD(bufferSize, buffer);
+ em->txandrx_PD(buffer_size_, current_buffer_);
// Convert status back to HW Interface
- p = buffer;
+ current = current_buffer_;
+ last = last_buffer_;
for (int i = 0; i < hw->numActuators; ++i)
{
- slaves[i]->convertState(hw->actuator[i].state, p);
- p += slaves[i]->commandSize + slaves[i]->statusSize;
+ slaves[i]->convertState(hw->actuator[i].state, current, last);
+ current += slaves[i]->commandSize + slaves[i]->statusSize;
+ last += slaves[i]->commandSize + slaves[i]->statusSize;
+ }
- }
+ unsigned char *tmp = current_buffer_;
+ current_buffer_ = last_buffer_;
+ last_buffer_ = tmp;
}
MotorControlBoard *
Modified: pkg/trunk/drivers/motor/ethercat_hardware/src/mk1001.cpp
===================================================================
--- pkg/trunk/drivers/motor/ethercat_hardware/src/mk1001.cpp 2008-07-29 01:35:41 UTC (rev 2228)
+++ pkg/trunk/drivers/motor/ethercat_hardware/src/mk1001.cpp 2008-07-29 02:04:47 UTC (rev 2229)
@@ -106,7 +106,7 @@
memcpy(buffer + sizeof(MK1001Status), &c, sizeof(c));
}
-void MK1001::convertState(ActuatorState &state, unsigned char *buffer)
+void MK1001::convertState(ActuatorState &state, unsigned char *buffer, unsigned char *not_used)
{
MK1001Status s;
MK1001Command c;
Modified: pkg/trunk/drivers/motor/ethercat_hardware/src/wg05.cpp
===================================================================
--- pkg/trunk/drivers/motor/ethercat_har...
[truncated message content] |
|
From: <hsu...@us...> - 2008-07-29 05:12:36
|
Revision: 2235
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=2235&view=rev
Author: hsujohnhsu
Date: 2008-07-29 05:12:45 +0000 (Tue, 29 Jul 2008)
Log Message:
-----------
added mission namespace specifications.
Modified Paths:
--------------
pkg/trunk/mechanism/mechanism_control/include/mechanism_control/base_control.h
pkg/trunk/mechanism/mechanism_control/include/mechanism_control/gazebo_control.h
pkg/trunk/mechanism/mechanism_control/include/mechanism_control/single_control.h
pkg/trunk/mechanism/mechanism_control/src/base_control.cpp
pkg/trunk/mechanism/mechanism_control/src/gazebo_control.cpp
pkg/trunk/mechanism/mechanism_control/src/single_control.cpp
pkg/trunk/robot_models/robot_mechanism_model/include/mechanism_model/robot.h
Modified: pkg/trunk/mechanism/mechanism_control/include/mechanism_control/base_control.h
===================================================================
--- pkg/trunk/mechanism/mechanism_control/include/mechanism_control/base_control.h 2008-07-29 04:56:32 UTC (rev 2234)
+++ pkg/trunk/mechanism/mechanism_control/include/mechanism_control/base_control.h 2008-07-29 05:12:45 UTC (rev 2235)
@@ -74,7 +74,7 @@
HardwareInterface *hw;
- Robot *r;
+ mechanism::Robot *r;
void initRobot();
Modified: pkg/trunk/mechanism/mechanism_control/include/mechanism_control/gazebo_control.h
===================================================================
--- pkg/trunk/mechanism/mechanism_control/include/mechanism_control/gazebo_control.h 2008-07-29 04:56:32 UTC (rev 2234)
+++ pkg/trunk/mechanism/mechanism_control/include/mechanism_control/gazebo_control.h 2008-07-29 05:12:45 UTC (rev 2235)
@@ -82,7 +82,7 @@
HardwareInterface *hardwareInterface;
- Robot *r;
+ mechanism::Robot *r;
void initRobot();
Modified: pkg/trunk/mechanism/mechanism_control/include/mechanism_control/single_control.h
===================================================================
--- pkg/trunk/mechanism/mechanism_control/include/mechanism_control/single_control.h 2008-07-29 04:56:32 UTC (rev 2234)
+++ pkg/trunk/mechanism/mechanism_control/include/mechanism_control/single_control.h 2008-07-29 05:12:45 UTC (rev 2235)
@@ -69,7 +69,7 @@
HardwareInterface *hw;
- Robot *r;
+ mechanism::Robot *r;
void initRobot();
Modified: pkg/trunk/mechanism/mechanism_control/src/base_control.cpp
===================================================================
--- pkg/trunk/mechanism/mechanism_control/src/base_control.cpp 2008-07-29 04:56:32 UTC (rev 2234)
+++ pkg/trunk/mechanism/mechanism_control/src/base_control.cpp 2008-07-29 05:12:45 UTC (rev 2235)
@@ -51,7 +51,7 @@
}
void BaseControl::initRobot(){
- r = new Robot("robot");
+ r = new mechanism::Robot("robot");
r->numJoints = NUM_JOINTS;
r->numLinks = NUM_JOINTS;
Modified: pkg/trunk/mechanism/mechanism_control/src/gazebo_control.cpp
===================================================================
--- pkg/trunk/mechanism/mechanism_control/src/gazebo_control.cpp 2008-07-29 04:56:32 UTC (rev 2234)
+++ pkg/trunk/mechanism/mechanism_control/src/gazebo_control.cpp 2008-07-29 05:12:45 UTC (rev 2235)
@@ -55,7 +55,7 @@
}
void GazeboMechanismControl::initRobot(){
- r = new Robot((char*)"robot");
+ r = new mechanism::Robot((char*)"robot");
r->numJoints = PR2::MAX_JOINTS;
r->numLinks = PR2::MAX_JOINTS;
Modified: pkg/trunk/mechanism/mechanism_control/src/single_control.cpp
===================================================================
--- pkg/trunk/mechanism/mechanism_control/src/single_control.cpp 2008-07-29 04:56:32 UTC (rev 2234)
+++ pkg/trunk/mechanism/mechanism_control/src/single_control.cpp 2008-07-29 05:12:45 UTC (rev 2235)
@@ -45,7 +45,7 @@
void SingleControl::init(HardwareInterface *hw){
this->hw = hw;
- r = new Robot((char*)"robot");
+ r = new mechanism::Robot((char*)"robot");
r->numJoints = NUM_JOINTS;
r->numTransmissions = NUM_JOINTS;
Modified: pkg/trunk/robot_models/robot_mechanism_model/include/mechanism_model/robot.h
===================================================================
--- pkg/trunk/robot_models/robot_mechanism_model/include/mechanism_model/robot.h 2008-07-29 04:56:32 UTC (rev 2234)
+++ pkg/trunk/robot_models/robot_mechanism_model/include/mechanism_model/robot.h 2008-07-29 05:12:45 UTC (rev 2235)
@@ -54,6 +54,8 @@
int numLinks;
Joint *joint; // TODO: delete
int numJoints;
+ int numTransmissions;
+ SimpleTransmission *transmission; // TODO: delete
std::vector<Joint*> joints_;
std::vector<Transmission*> transmissions_;
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <stu...@us...> - 2008-07-29 17:14:55
|
Revision: 2244
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=2244&view=rev
Author: stuglaser
Date: 2008-07-29 17:15:03 +0000 (Tue, 29 Jul 2008)
Log Message:
-----------
Added initXml method to Controller base class. Calling it in mechanism control.
Modified Paths:
--------------
pkg/trunk/controllers/genericControllers/include/genericControllers/Controller.h
pkg/trunk/mechanism/mechanism_control/include/mechanism_control/mechanism_control.h
pkg/trunk/mechanism/mechanism_control/src/mechanism_control.cpp
Modified: pkg/trunk/controllers/genericControllers/include/genericControllers/Controller.h
===================================================================
--- pkg/trunk/controllers/genericControllers/include/genericControllers/Controller.h 2008-07-29 07:41:01 UTC (rev 2243)
+++ pkg/trunk/controllers/genericControllers/include/genericControllers/Controller.h 2008-07-29 17:15:03 UTC (rev 2244)
@@ -9,8 +9,9 @@
*/
/***************************************************/
+#include <mechanism_model/robot.h>
+
class TiXmlElement;
-class Robot;
namespace controller
{
@@ -46,7 +47,7 @@
virtual ~Controller() {}
virtual void update(void) {}
virtual void init(void) {}
- virtual void initXml(Robot *robot, TiXmlElement *config) {}
+ virtual void initXml(mechanism::Robot *robot, TiXmlElement *config) {}
private:
};
Modified: pkg/trunk/mechanism/mechanism_control/include/mechanism_control/mechanism_control.h
===================================================================
--- pkg/trunk/mechanism/mechanism_control/include/mechanism_control/mechanism_control.h 2008-07-29 07:41:01 UTC (rev 2243)
+++ pkg/trunk/mechanism/mechanism_control/include/mechanism_control/mechanism_control.h 2008-07-29 17:15:03 UTC (rev 2244)
@@ -37,19 +37,19 @@
#include <rosthread/mutex.h>
#include <genericControllers/Controller.h>
-typedef controller::Controller* (*ControllerAllocator)(const std::string&);
+typedef controller::Controller* (*ControllerAllocator)();
class MechanismControl {
public:
MechanismControl(HardwareInterface *hw);
~MechanismControl();
- bool init();
+ bool init(TiXmlElement* config);
void update(); //Must be realtime safe
bool registerActuator(const std::string &name, int index);
void registerControllerType(const std::string& type, ControllerAllocator f);
- bool spawnController(const char* type, const char* ns);
+ bool spawnController(const char* type, TiXmlElement* config);
Actuator* getActuator(const std::string& name);
Modified: pkg/trunk/mechanism/mechanism_control/src/mechanism_control.cpp
===================================================================
--- pkg/trunk/mechanism/mechanism_control/src/mechanism_control.cpp 2008-07-29 07:41:01 UTC (rev 2243)
+++ pkg/trunk/mechanism/mechanism_control/src/mechanism_control.cpp 2008-07-29 17:15:03 UTC (rev 2244)
@@ -2,32 +2,34 @@
///////////////////////////////////////////////////////////////////////////////
// Copyright (C) 2008, Willow Garage, Inc.
//
-// Redistribution and use in source and binary forms, with or without
+// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
-// * Redistributions of source code must retain the above copyright notice,
+// * Redistributions of source code must retain the above copyright notice,
// this list of conditions and the following disclaimer.
-// * Redistributions in binary form must reproduce the above copyright
-// notice, this list of conditions and the following disclaimer in the
+// * Redistributions in binary form must reproduce the above copyright
+// notice, this list of conditions and the following disclaimer in the
// documentation and/or other materials provided with the distribution.
-// * Neither the name of Willow Garage, Inc. nor the names of its
-// contributors may be used to endorse or promote products derived from
+// * Neither the name of Willow Garage, Inc. nor the names of its
+// contributors may be used to endorse or promote products derived from
// this software without specific prior written permission.
-//
-// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
-// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
-// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
-// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
-// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
-// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
-// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
-// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
-// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
-// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
//////////////////////////////////////////////////////////////////////////////
#include "mechanism_control/mechanism_control.h"
+using namespace mechanism;
+
MechanismControl::MechanismControl(HardwareInterface *hw)
: initialized_(0), model_((char*)"robot"), hw_(hw)
{
@@ -46,48 +48,90 @@
return true;
}
-bool MechanismControl::init() { // TODO: should take in a node of xml?
+bool MechanismControl::init(TiXmlElement* config) {
+
+ bool successful = true;
+
// Creates:
// - joints
// - transmissions
- return true;
+ TiXmlElement *elt;
+
+ // Constructs the joints
+ std::map<std::string, Joint*> joint_map;
+ for (elt = config->FirstChildElement("joint"); elt; elt = elt->NextSiblingElement("joint"))
+ {
+ Joint *j = new Joint;
+ model_.joints_.push_back(j);
+ joint_map[elt->Attribute("name")] = j;
+ j->jointLimitMin = atof(elt->FirstChildElement("limitMin")->GetText());
+ j->jointLimitMax = atof(elt->FirstChildElement("limitMax")->GetText());
+ j->effortLimit = atof(elt->FirstChildElement("effortLimit")->GetText());
+ j->velocityLimit = atof(elt->FirstChildElement("velocityLimit")->GetText());
+ }
+
+ // Constructs the transmissions
+ elt = config->FirstChildElement("transmission");
+ for (; elt; elt = elt->NextSiblingElement("transmission"))
+ {
+ if (0 == strcmp("SimpleTransmission", elt->Attribute("type")))
+ {
+ // Looks up the joint and the actuator used by the transmission.
+ std::map<std::string,Joint*>::iterator joint_it =
+ joint_map.find(elt->FirstChildElement("joint")->Attribute("name"));
+ ActuatorMap::iterator actuator_it =
+ actuators_.find(elt->FirstChildElement("actuator")->Attribute("name"));
+ if (joint_it == joint_map.end())
+ {
+ // TODO: report: The joint was not declared in the XML file
+ continue;
+ }
+ if (actuator_it == actuators_.end())
+ {
+ // TODO: report: The actuator was not registered with mechanism control.
+ continue;
+ }
+
+ Transmission *tr = new SimpleTransmission
+ (joint_it->second, &hw_->actuator[actuator_it->second],
+ atof(elt->FirstChildElement("mechanicalReduction")->Value()),
+ atof(elt->FirstChildElement("motorTorqueConstant")->Value()),
+ atof(elt->FirstChildElement("pulsesPerRevolution")->Value()));
+ model_.transmissions_.push_back(tr);
+ }
+ else
+ {
+ // TODO: report: Unknown transmission type
+ successful = false;
+ }
+ }
+
+ initialized_ = true;
+ return successful;
}
// Must be realtime safe.
void MechanismControl::update() {
- // Propogates actuator information into the robot model.
- // Propogates through the robot model.
- // Calls update on all of the plugins.
- // Performs safety checks on the commands.
- // Propogates commands back into the actuators.
+ // Propagates through the robot model.
+ for (unsigned int i = 0; i < model_.transmissions_.size(); ++i)
+ model_.transmissions_[i]->propagatePosition();
-#if 0
- //Clear actuator commands
+ // TODO: update KDL model with new joint position/velocities
- //Process robot model transmissions
- for(int i = 0; i < r->numTransmissions; i++){
- r->transmission[i].propagatePosition();
- }
-
- //update KDL model with new joint position/velocities
- //
-
//update all controllers
- for(int i = 0; i < MAX_NUM_CONTROLLERS; i++){
- if(controller[i] != NULL){
- controller[i]->update();
- }
+ for(int i = 0; i < MAX_NUM_CONTROLLERS; ++i){
+ if(controllers_[i] != NULL)
+ controllers_[i]->update();
}
- for(int i = 0; i < r->numJoints; i++){
- r->joint[i].enforceLimits();
- }
+ // Performs safety checks on the commands.
+ for (unsigned int i = 0; i < model_.joints_.size(); ++i)
+ model_.joints_[i]->enforceLimits();
- for(int i = 0; i < r->numTransmissions; i++){
- r->transmission[i].propagateEffort();
- }
-#endif
+ // Propagates commands back into the actuators.
+ for (unsigned int i = 0; i < model_.transmissions_.size(); ++i)
+ model_.transmissions_[i]->propagateEffort();
}
@@ -96,10 +140,12 @@
controller_library_.insert(std::pair<std::string,ControllerAllocator>(type, f));
}
-bool MechanismControl::spawnController(const char *type, const char *ns){
+bool MechanismControl::spawnController(const char *type, TiXmlElement *config)
+{
controller::Controller *c;
ControllerAllocator f = controller_library_[type];
- c = f(ns);
+ c = f();
+ c->initXml(&model_, config);
//At this point, the controller is fully initialized and has created the ROS interface, etc.
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <stu...@us...> - 2008-07-29 19:54:11
|
Revision: 2265
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=2265&view=rev
Author: stuglaser
Date: 2008-07-29 19:54:19 +0000 (Tue, 29 Jul 2008)
Log Message:
-----------
Looking up actuators and joints by name is now included in the robot model
(instead of in mechanism control).
Modified Paths:
--------------
pkg/trunk/mechanism/mechanism_control/include/mechanism_control/mechanism_control.h
pkg/trunk/mechanism/mechanism_control/src/mechanism_control.cpp
pkg/trunk/robot_models/robot_mechanism_model/include/mechanism_model/robot.h
Modified: pkg/trunk/mechanism/mechanism_control/include/mechanism_control/mechanism_control.h
===================================================================
--- pkg/trunk/mechanism/mechanism_control/include/mechanism_control/mechanism_control.h 2008-07-29 19:53:34 UTC (rev 2264)
+++ pkg/trunk/mechanism/mechanism_control/include/mechanism_control/mechanism_control.h 2008-07-29 19:54:19 UTC (rev 2265)
@@ -51,12 +51,8 @@
void registerControllerType(const std::string& type, ControllerAllocator f);
bool spawnController(const char* type, TiXmlElement* config);
- Actuator* getActuator(const std::string& name);
-
private:
bool initialized_;
- typedef std::map<std::string,int> ActuatorMap;
- ActuatorMap actuators_;
mechanism::Robot model_;
HardwareInterface *hw_;
Modified: pkg/trunk/mechanism/mechanism_control/src/mechanism_control.cpp
===================================================================
--- pkg/trunk/mechanism/mechanism_control/src/mechanism_control.cpp 2008-07-29 19:53:34 UTC (rev 2264)
+++ pkg/trunk/mechanism/mechanism_control/src/mechanism_control.cpp 2008-07-29 19:54:19 UTC (rev 2265)
@@ -40,22 +40,20 @@
MechanismControl::~MechanismControl() {
}
-bool MechanismControl::registerActuator(const std::string &name, int index) {
+bool MechanismControl::registerActuator(const std::string &name, int index)
+{
if (initialized_)
return false;
- actuators_.insert(ActuatorMap::value_type(name, index));
+ model_.actuators_lookup_.insert(Robot::IndexMap::value_type(name, index));
return true;
}
-bool MechanismControl::init(TiXmlElement* config) {
-
+bool MechanismControl::init(TiXmlElement* config)
+{
bool successful = true;
- // Creates:
- // - joints
- // - transmissions
TiXmlElement *elt;
// Constructs the joints
@@ -64,7 +62,8 @@
{
Joint *j = new Joint;
model_.joints_.push_back(j);
- joint_map[elt->Attribute("name")] = j;
+ model_.joints_lookup_.insert(
+ Robot::IndexMap::value_type(elt->Attribute("name"), model_.joints_.size() - 1));
j->jointLimitMin = atof(elt->FirstChildElement("limitMin")->GetText());
j->jointLimitMax = atof(elt->FirstChildElement("limitMax")->GetText());
j->effortLimit = atof(elt->FirstChildElement("effortLimit")->GetText());
@@ -78,23 +77,23 @@
if (0 == strcmp("SimpleTransmission", elt->Attribute("type")))
{
// Looks up the joint and the actuator used by the transmission.
- std::map<std::string,Joint*>::iterator joint_it =
- joint_map.find(elt->FirstChildElement("joint")->Attribute("name"));
- ActuatorMap::iterator actuator_it =
- actuators_.find(elt->FirstChildElement("actuator")->Attribute("name"));
- if (joint_it == joint_map.end())
+ Robot::IndexMap::iterator joint_it =
+ model_.joints_lookup_.find(elt->FirstChildElement("joint")->Attribute("name"));
+ Robot::IndexMap::iterator actuator_it =
+ model_.actuators_lookup_.find(elt->FirstChildElement("actuator")->Attribute("name"));
+ if (joint_it == model_.joints_lookup_.end())
{
// TODO: report: The joint was not declared in the XML file
continue;
}
- if (actuator_it == actuators_.end())
+ if (actuator_it == model_.actuators_lookup_.end())
{
// TODO: report: The actuator was not registered with mechanism control.
continue;
}
Transmission *tr = new SimpleTransmission
- (joint_it->second, &hw_->actuator[actuator_it->second],
+ (model_.joints_[joint_it->second], &hw_->actuator[actuator_it->second],
atof(elt->FirstChildElement("mechanicalReduction")->Value()),
atof(elt->FirstChildElement("motorTorqueConstant")->Value()),
atof(elt->FirstChildElement("pulsesPerRevolution")->Value()));
Modified: pkg/trunk/robot_models/robot_mechanism_model/include/mechanism_model/robot.h
===================================================================
--- pkg/trunk/robot_models/robot_mechanism_model/include/mechanism_model/robot.h 2008-07-29 19:53:34 UTC (rev 2264)
+++ pkg/trunk/robot_models/robot_mechanism_model/include/mechanism_model/robot.h 2008-07-29 19:54:19 UTC (rev 2265)
@@ -32,38 +32,62 @@
#define ROBOT_H
#include <vector>
+#include <map>
+#include <string>
#include "mechanism_model/link.h"
#include "mechanism_model/joint.h"
#include "mechanism_model/transmission.h"
#include "hw_interface/hardware_interface.h"
namespace mechanism {
- class Robot{
- public:
- Robot(char *ns){}
- ~Robot()
- {
- std::vector<Transmission *>::size_type t;
- for (t = 0; t < transmissions_.size(); ++t)
- delete transmissions_[t];
- std::vector<Joint *>::size_type j;
- for (j = 0; j < joints_.size(); ++j)
- delete joints_[j];
- }
- char *name;
- Link *link;
- int numLinks;
- Joint *joint; // TODO: delete
- int numJoints;
- int numTransmissions;
- SimpleTransmission *transmission; // TODO: delete
+class Robot{
+public:
+ Robot(char *ns){}
+ ~Robot()
+ {
+ std::vector<Transmission *>::size_type t;
+ for (t = 0; t < transmissions_.size(); ++t)
+ delete transmissions_[t];
+ std::vector<Joint *>::size_type j;
+ for (j = 0; j < joints_.size(); ++j)
+ delete joints_[j];
+ }
- std::vector<Joint*> joints_;
- std::vector<Transmission*> transmissions_;
+ char *name;
+ Link *link;
+ int numLinks;
+ Joint *joint; // TODO: delete
+ int numJoints;
+ int numTransmissions;
+ SimpleTransmission *transmission; // TODO: delete
- HardwareInterface *hw_;
- };
+ std::vector<Joint*> joints_;
+ std::vector<Transmission*> transmissions_;
+
+ // Supports looking up joints and actuators by name. The IndexMap
+ // structure maps the name of the item to its index in the vectors.
+ typedef std::map<std::string,int> IndexMap;
+ IndexMap joints_lookup_;
+ IndexMap actuators_lookup_;
+ Joint* getJoint(const std::string &name)
+ {
+ IndexMap::iterator it = joints_lookup_.find(name);
+ if (it == joints_lookup_.end())
+ return NULL;
+ return joints_[it->second];
+ }
+ Actuator* getActuator(const std::string &name)
+ {
+ IndexMap::iterator it = actuators_lookup_.find(name);
+ if (it == actuators_lookup_.end())
+ return NULL;
+ return &hw_->actuator[it->second];
+ }
+
+ HardwareInterface *hw_;
+};
+
}
#endif
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <adv...@us...> - 2008-07-30 05:46:05
|
Revision: 2302
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=2302&view=rev
Author: advaitjain
Date: 2008-07-30 05:46:12 +0000 (Wed, 30 Jul 2008)
Log Message:
-----------
added a new position sensor in gazebo_sensors and propagated it
all the way through. We can attach this sensor to any object
in the simulation and use it to get the the objects pose.
Modified Paths:
--------------
pkg/trunk/drivers/robot/pr2/libpr2API/include/libpr2API/pr2API.h
pkg/trunk/drivers/robot/pr2/libpr2API/src/pr2API.cpp
pkg/trunk/drivers/robot/pr2/libpr2HW/include/libpr2HW/pr2HW.h
pkg/trunk/drivers/robot/pr2/libpr2HW/src/pr2HW.cpp
pkg/trunk/drivers/simulator/gazebo_sensors/include/gazebo_sensors/gazebo_sensors.h
pkg/trunk/drivers/simulator/gazebo_sensors/src/gazebo_sensors.cpp
pkg/trunk/manip/overhead_grasp_behavior/src/overhead_grasp.cc
pkg/trunk/robot_descriptions/gazebo_robot_description/world/pr2.model
pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_tablegrasp.world
pkg/trunk/simulators/rosgazebo/include/RosGazeboNode/RosGazeboNode.h
pkg/trunk/simulators/rosgazebo/src/RosGazeboNode.cpp
pkg/trunk/util/kinematics/libKDL/src/kdl_kinematics.cpp
pkg/trunk/util/kinematics/libKDL/test/pr2_kin_test.cpp
pkg/trunk/vision/cam_viewer/cam_viewer.cpp
Modified: pkg/trunk/drivers/robot/pr2/libpr2API/include/libpr2API/pr2API.h
===================================================================
--- pkg/trunk/drivers/robot/pr2/libpr2API/include/libpr2API/pr2API.h 2008-07-30 02:27:50 UTC (rev 2301)
+++ pkg/trunk/drivers/robot/pr2/libpr2API/include/libpr2API/pr2API.h 2008-07-30 05:46:12 UTC (rev 2302)
@@ -553,6 +553,11 @@
public: PR2_ERROR_CODE GetBasePositionActual(double* x, double* y, double *z, double *roll, double *pitch, double *yaw);
/*! \fn
+ \brief Retrieve position and orientation of an object.
+ */
+ public: PR2_ERROR_CODE GetObjectPositionActual(double* x, double* y, double *z, double *roll, double *pitch, double *yaw);
+
+ /*! \fn
\brief - Run the robot
*/
public: PR2_ERROR_CODE RunRobot();
Modified: pkg/trunk/drivers/robot/pr2/libpr2API/src/pr2API.cpp
===================================================================
--- pkg/trunk/drivers/robot/pr2/libpr2API/src/pr2API.cpp 2008-07-30 02:27:50 UTC (rev 2301)
+++ pkg/trunk/drivers/robot/pr2/libpr2API/src/pr2API.cpp 2008-07-30 05:46:12 UTC (rev 2302)
@@ -918,7 +918,13 @@
return PR2_ALL_OK;
};
+PR2_ERROR_CODE PR2Robot::GetObjectPositionActual(double* x, double* y, double *z, double *roll, double *pitch, double *yaw)
+{
+ hw.GetObjectPositionGroundTruth(x,y,z,roll,pitch,yaw);
+ return PR2_ALL_OK;
+};
+
PR2_ERROR_CODE PR2Robot::GetLeftGripperCmd(double *gap,double *force)
{
hw.GetGripperCmd((PR2_MODEL_ID)PR2::PR2_LEFT_GRIPPER,gap,force);
Modified: pkg/trunk/drivers/robot/pr2/libpr2HW/include/libpr2HW/pr2HW.h
===================================================================
--- pkg/trunk/drivers/robot/pr2/libpr2HW/include/libpr2HW/pr2HW.h 2008-07-30 02:27:50 UTC (rev 2301)
+++ pkg/trunk/drivers/robot/pr2/libpr2HW/include/libpr2HW/pr2HW.h 2008-07-30 05:46:12 UTC (rev 2302)
@@ -320,6 +320,11 @@
public: PR2_ERROR_CODE GetBasePositionGroundTruth(double* x, double* y, double *z, double *roll, double *pitch, double *yaw);
/*! \fn
+ \brief - Get ground truth object position
+ */
+ public: PR2_ERROR_CODE GetObjectPositionGroundTruth(double* x, double* y, double *z, double *roll, double *pitch, double *yaw);
+
+ /*! \fn
\brief Wait for Gazebo to update
TODO: put this in UpdateHW()?
*/
Modified: pkg/trunk/drivers/robot/pr2/libpr2HW/src/pr2HW.cpp
===================================================================
--- pkg/trunk/drivers/robot/pr2/libpr2HW/src/pr2HW.cpp 2008-07-30 02:27:50 UTC (rev 2301)
+++ pkg/trunk/drivers/robot/pr2/libpr2HW/src/pr2HW.cpp 2008-07-30 05:46:12 UTC (rev 2302)
@@ -46,6 +46,7 @@
gazebo::PositionIface *pr2LeftWristIface;
gazebo::PositionIface *pr2RightWristIface;
gazebo::PositionIface *pr2BaseIface;
+gazebo::PositionIface *posObjectIface;
gazebo::CameraIface *pr2WristCameraLeftIface;
gazebo::CameraIface *pr2WristCameraRightIface;
gazebo::CameraIface *pr2ForearmCameraLeftIface;
@@ -96,6 +97,8 @@
pr2RightWristIface = new gazebo::PositionIface();
pr2BaseIface = new gazebo::PositionIface();
+ posObjectIface = new gazebo::PositionIface();
+
int serverId = 0;
/// Connect to the libgazebo server
@@ -307,6 +310,17 @@
pr2BaseIface = NULL;
}
+ try
+ {
+ posObjectIface->Open(client, "p3d_object_position");
+ }
+ catch (std::string e)
+ {
+ std::cout << "Gazebo error: Unable to connect to the object position interface\n"
+ << e << "\n";
+ posObjectIface = NULL;
+ }
+
std::cout << "initial HW reads\n" << std::endl;
// fill in actuator data
for (int id = 0; id < PR2::HEAD_PTZ_R_TILT; id++)
@@ -1001,6 +1015,22 @@
return PR2_ALL_OK;
};
+PR2_ERROR_CODE PR2HW::GetObjectPositionGroundTruth(double* x, double* y, double *z, double *roll, double *pitch, double *yaw)
+{
+ if(posObjectIface == NULL)
+ return PR2_ALL_OK;
+
+ posObjectIface->Lock(1);
+ *x = posObjectIface->data->pose.pos.x;
+ *y = posObjectIface->data->pose.pos.y;
+ *z = posObjectIface->data->pose.pos.z;
+ *roll = posObjectIface->data->pose.roll;
+ *pitch = posObjectIface->data->pose.pitch;
+ *yaw = posObjectIface->data->pose.yaw;
+ posObjectIface->Unlock();
+ return PR2_ALL_OK;
+};
+
PR2_ERROR_CODE PR2HW::ClientWait()
{
// block until simulator update.
Modified: pkg/trunk/drivers/simulator/gazebo_sensors/include/gazebo_sensors/gazebo_sensors.h
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_sensors/include/gazebo_sensors/gazebo_sensors.h 2008-07-30 02:27:50 UTC (rev 2301)
+++ pkg/trunk/drivers/simulator/gazebo_sensors/include/gazebo_sensors/gazebo_sensors.h 2008-07-30 05:46:12 UTC (rev 2302)
@@ -78,7 +78,9 @@
gazebo::PositionIface *pr2RightWristIface;
gazebo::PositionIface *pr2BaseIface;
+ gazebo::PositionIface *posObjectIface;
+
/*! \fn
\brief Constructor
*/
@@ -170,6 +172,10 @@
*/
public: PR2::PR2_ERROR_CODE GetWristPoseGroundTruth(PR2::PR2_MODEL_ID id, double *x, double *y, double *z, double *roll, double *pitch, double *yaw);
+ /*! \fn
+ \brief - 3D position sensor can be attached to any object to get its position and orientation.
+ */
+ public: PR2::PR2_ERROR_CODE GetObjectPositionGroundTruth(double* x, double* y, double *z, double *roll, double *pitch, double *yaw);
/*! \fn
\brief Wait for Gazebo to update
Modified: pkg/trunk/drivers/simulator/gazebo_sensors/src/gazebo_sensors.cpp
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_sensors/src/gazebo_sensors.cpp 2008-07-30 02:27:50 UTC (rev 2301)
+++ pkg/trunk/drivers/simulator/gazebo_sensors/src/gazebo_sensors.cpp 2008-07-30 05:46:12 UTC (rev 2302)
@@ -33,6 +33,8 @@
pr2LeftWristIface = new gazebo::PositionIface();
pr2RightWristIface = new gazebo::PositionIface();
pr2BaseIface = new gazebo::PositionIface();
+
+ posObjectIface = new gazebo::PositionIface();
}
GazeboSensors::~GazeboSensors()
@@ -158,7 +160,18 @@
pr2BaseIface = NULL;
}
+ try
+ {
+ posObjectIface->Open(client, "p3d_object_position");
+ }
+ catch (std::string e)
+ {
+ std::cout << "Gazebo error: Unable to connect to the object position interface\n"
+ << e << "\n";
+ posObjectIface = NULL;
+ }
+
return PR2::PR2_ALL_OK;
};
@@ -352,6 +365,19 @@
return PR2::PR2_ALL_OK;
};
+PR2::PR2_ERROR_CODE GazeboSensors::GetObjectPositionGroundTruth(double* x, double* y, double *z, double *roll, double *pitch, double *yaw)
+{
+ posObjectIface->Lock(1);
+ *x = posObjectIface->data->pose.pos.x;
+ *y = posObjectIface->data->pose.pos.y;
+ *z = posObjectIface->data->pose.pos.z;
+ *roll = posObjectIface->data->pose.roll;
+ *pitch = posObjectIface->data->pose.pitch;
+ *yaw = posObjectIface->data->pose.yaw;
+ posObjectIface->Unlock();
+ return PR2::PR2_ALL_OK;
+};
+
PR2::PR2_ERROR_CODE GazeboSensors::ClientWait()
{
// block until simulator update.
Modified: pkg/trunk/manip/overhead_grasp_behavior/src/overhead_grasp.cc
===================================================================
--- pkg/trunk/manip/overhead_grasp_behavior/src/overhead_grasp.cc 2008-07-30 02:27:50 UTC (rev 2301)
+++ pkg/trunk/manip/overhead_grasp_behavior/src/overhead_grasp.cc 2008-07-30 05:46:12 UTC (rev 2302)
@@ -6,91 +6,110 @@
#include <libpr2API/pr2API.h>
#include <pr2_msgs/EndEffectorState.h>
+#include <std_msgs/Point3DFloat32.h>
#include <std_msgs/Float64.h>
#include <std_msgs/PR2Arm.h>
#include <unistd.h>
using namespace KDL;
-// coordinates of arm's origin in gazebo/arm coordinate frame (the axes of both frames are parallel)
-Vector gazebo_to_arm_vector = Vector(1.040-0.7987,-0.15,0.8269);
-void quit(int sig);
-void keyboardLoop(ros::node &n);
+class OverheadGrasper : public ros::node
+{
+ public:
+ // coordinates of arm's origin in gazebo/arm coordinate frame (the axes of both frames are parallel)
+ Vector gazebo_to_arm_vector;
+ std_msgs::Point3DFloat32 objectPosMsg;
+ public:
+ OverheadGrasper(void) : ros::node("overhead_grasper")
+ {
+ advertise<std_msgs::PR2Arm>("cmd_rightarmconfig");
+ advertise<pr2_msgs::EndEffectorState>("right_pr2arm_set_end_effector");
+ advertise<std_msgs::Float64>("interpolate_step_size");
+ advertise<std_msgs::Float64>("interpolate_wait_time");
+ subscribe("object_position", objectPosMsg, &OverheadGrasper::objectPosition_cb);
-void positionArmCartesian(ros::node &n, Vector v, Rotation r)
-{
- pr2_msgs::EndEffectorState efs;
- efs.set_rot_size(9);
- efs.set_trans_size(3);
- for(int i = 0; i < 9; i++) {
- efs.rot[i] = r.data[i];
- }
- for(int i = 0; i < 3; i++) {
- efs.trans[i] = v.data[i];
- }
+ gazebo_to_arm_vector = Vector(1.040-0.7987,-0.15,0.8269);
+ }
- n.publish("right_pr2arm_set_end_effector",efs);
-}
+ void positionArmCartesian(Vector v, Rotation r)
+ {
+ pr2_msgs::EndEffectorState efs;
+ efs.set_rot_size(9);
+ efs.set_trans_size(3);
+ for(int i = 0; i < 9; i++) {
+ efs.rot[i] = r.data[i];
+ }
+ for(int i = 0; i < 3; i++) {
+ efs.trans[i] = v.data[i];
+ }
+ publish("right_pr2arm_set_end_effector",efs);
+ }
-void overheadGraspPosture(ros::node &n)
-{
- std_msgs::PR2Arm rightarm;
- rightarm.turretAngle = deg2rad*-20;
- rightarm.shoulderLiftAngle = deg2rad*-20;
- rightarm.upperarmRollAngle = deg2rad*180;
- rightarm.elbowAngle = deg2rad*-30;
- rightarm.forearmRollAngle = 0;
- rightarm.wristPitchAngle = 0;
- rightarm.wristRollAngle = 0;
- rightarm.gripperForceCmd = 0;
- rightarm.gripperGapCmd = 0.2;
- n.publish("cmd_rightarmconfig",rightarm);
-}
+ void overheadGraspPosture()
+ {
+ std_msgs::PR2Arm rightarm;
+ rightarm.turretAngle = deg2rad*-20;
+ rightarm.shoulderLiftAngle = deg2rad*-20;
+ rightarm.upperarmRollAngle = deg2rad*180;
+ rightarm.elbowAngle = deg2rad*-30;
+ rightarm.forearmRollAngle = 0;
+ rightarm.wristPitchAngle = 0;
+ rightarm.wristRollAngle = 0;
+ rightarm.gripperForceCmd = 0;
+ rightarm.gripperGapCmd = 0.2;
+ publish("cmd_rightarmconfig",rightarm);
+ }
-// Vector v is in gazebo coordinate frame.
-void positionEyecamOverObject(ros::node &n, Vector v)
-{
- Vector v_arm = v - gazebo_to_arm_vector;
- v_arm.data[2] += 0.3; // I want end effector to be 0.5m above object.
- Rotation r = Rotation::RotY(DTOR(90)); // look down vertically
- cout<<"Going to: "<<v_arm<<endl;
- positionArmCartesian(n, v_arm, r);
-}
+ // Vector v is in gazebo coordinate frame.
+ void positionEyecamOverObject(Vector v)
+ {
+ Vector v_arm = v - gazebo_to_arm_vector;
+ v_arm.data[2] += 0.4; // I want end effector to be 0.5m above object.
+ Rotation r = Rotation::RotY(DTOR(90)); // look down vertically
+ cout<<"Going to: "<<v_arm<<endl;
+ positionArmCartesian(v_arm, r);
+ }
+ void objectPosition_cb(void)
+ {
+ }
+
+};
+
+
+void quit(int sig);
+void keyboardLoop(OverheadGrasper &n);
+
+
int main(int argc, char **argv)
{
ros::init(argc, argv);
- ros::node mynode("overhead_grasp");
+ OverheadGrasper ohGrasper;
- mynode.advertise<std_msgs::PR2Arm>("cmd_rightarmconfig");
- mynode.advertise<pr2_msgs::EndEffectorState>("right_pr2arm_set_end_effector");
- mynode.advertise<std_msgs::Float64>("interpolate_step_size");
- mynode.advertise<std_msgs::Float64>("interpolate_wait_time");
-
std_msgs::Float64 float64_msg;
sleep(1);
double interpolate_step_size = 0.05;
float64_msg.data = interpolate_step_size;
- mynode.publish("interpolate_step_size", float64_msg);
+ ohGrasper.publish("interpolate_step_size", float64_msg);
double interpolate_wait_time = 1.0;
float64_msg.data = interpolate_wait_time;
- mynode.publish("interpolate_wait_time", float64_msg);
+ ohGrasper.publish("interpolate_wait_time", float64_msg);
//---- now for the keyboard driven state machine ------
while(1)
{
signal(SIGINT,quit);
- keyboardLoop(mynode);
+ keyboardLoop(ohGrasper);
}
return 0;
@@ -110,7 +129,7 @@
}
-void keyboardLoop(ros::node &n)
+void keyboardLoop(OverheadGrasper &n)
{
char c;
@@ -139,14 +158,17 @@
switch(c)
{
case '1':
- overheadGraspPosture(n);
+ n.overheadGraspPosture();
break;
case '2':
{
- Vector v(.835,-.45,0.70);
- positionEyecamOverObject(n, v);
+ Vector v(n.objectPosMsg.x,n.objectPosMsg.y,n.objectPosMsg.z);
+ n.positionEyecamOverObject(v);
}
break;
+ case '3':
+ printf("x: %.4f y: %.4f z: %.4f\n", n.objectPosMsg.x, n.objectPosMsg.y, n.objectPosMsg.z);
+ break;
default:
break;
}
Modified: pkg/trunk/robot_descriptions/gazebo_robot_description/world/pr2.model
===================================================================
--- pkg/trunk/robot_descriptions/gazebo_robot_description/world/pr2.model 2008-07-30 02:27:50 UTC (rev 2301)
+++ pkg/trunk/robot_descriptions/gazebo_robot_description/world/pr2.model 2008-07-30 05:46:12 UTC (rev 2302)
@@ -359,7 +359,7 @@
</body:box>
<body:empty name="forearm_cam_right_body">
- <xyz> 0.90225 -0.15 0.8369 </xyz>
+ <xyz> 0.92225 -0.15 0.8369 </xyz>
<rpy> 0.00 -45.0 0.00 </rpy>
<sensor:camera name="forearm_cam_right_sensor">
<imageSize>640 480</imageSize>
Modified: pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_tablegrasp.world
===================================================================
--- pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_tablegrasp.world 2008-07-30 02:27:50 UTC (rev 2301)
+++ pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_tablegrasp.world 2008-07-30 05:46:12 UTC (rev 2302)
@@ -47,6 +47,7 @@
<material>Gazebo/CloudySky</material>
</sky>
<gazeboPath>media</gazeboPath>
+ <shadowTechnique>none</shadowTechnique>
<grid>off</grid>
<maxUpdateRate>100</maxUpdateRate>
</rendering:ogre>
@@ -147,40 +148,56 @@
</body:box>
</model:physical>
- <!-- The small cylinder "cup" -->
- <model:physical name="cylinder1_model">
- <xyz> 2.5 0.0 0.9</xyz>
+ <!-- The small box "cup" -->
+ <model:physical name="object1_model">
+ <xyz> 0.835 -0.55 0.95</xyz>
+ <rpy> 0.0 0.0 30.0</rpy>
+ <body:box name="object1_body">
+ <geom:box name="object1_geom">
+ <kp>100000000.0</kp>
+ <kd>0.1</kd>
+ <mesh>default</mesh>
+ <size>0.1 0.03 0.03</size>
+ <mass> 0.05</mass>
+ <visual>
+ <size> 0.1 0.030 0.03</size>
+ <material>Gazebo/PioneerBody</material>
+ <mesh>unit_box</mesh>
+ </visual>
+ </geom:box>
+ </body:box>
+
+ <controller:P3D name="p3d_object_controller" plugin="libP3D.so">
+ <updateRate>100.0</updateRate>
+ <bodyName>object1_body</bodyName>
+ <interface:position name="p3d_object_position"/>
+ </controller:P3D>
+
+ </model:physical>
+
+
+
+ <!--
+ <model:physical name="object1_model">
+ <xyz> 1.035 -0.15 0.55</xyz>
<rpy> 0.0 0.0 0.0</rpy>
- <body:cylinder name="cylinder1_body">
- <geom:cylinder name="cylinder1_geom">
+ <body:box name="object1_body">
+ <geom:box name="object1_geom">
<kp>100000000.0</kp>
<kd>0.1</kd>
<mesh>default</mesh>
- <size>0.025 0.075</size>
+ <size>0.01 0.01 0.01</size>
<mass> 0.05</mass>
<visual>
- <size> 0.05 0.05 0.075</size>
+ <size> 0.01 0.010 0.01</size>
<material>Gazebo/PioneerBody</material>
- <mesh>unit_cylinder</mesh>
- </visual>
- </geom:cylinder>
- <geom:box name="cylinder1_base_geom">
- <kp>100000000.0</kp>
- <kd>0.1</kd>
- <mesh>default</mesh>
- <xyz>0.0 0.0 -0.033</xyz>
- <size>0.05 0.05 0.01</size>
- <mass> 0.01</mass>
- <visual>
- <size> 0.05 0.05 0.01</size>
- <material>Gazebo/Fish</material>
<mesh>unit_box</mesh>
</visual>
</geom:box>
- </body:cylinder>
+ </body:box>
</model:physical>
+ -->
-
<!-- The small ball -->
<model:physical name="sphere1_model">
<xyz> 2.5 -2.8 1.0</xyz>
@@ -293,12 +310,12 @@
<model:renderable name="directional_white">
<light>
<type>directional</type>
- <direction>0 -0.5 -0.5</direction>
+ <direction>0 0.0 -1.0</direction>
<diffuseColor>0.4 0.4 0.4</diffuseColor>
<specularColor>0.0 0.0 0.0</specularColor>
<attenuation>1 0.0 1.0 0.4</attenuation>
</light>
</model:renderable>
-
</gazebo:world>
+
Modified: pkg/trunk/simulators/rosgazebo/include/RosGazeboNode/RosGazeboNode.h
===================================================================
--- pkg/trunk/simulators/rosgazebo/include/RosGazeboNode/RosGazeboNode.h 2008-07-30 02:27:50 UTC (rev 2301)
+++ pkg/trunk/simulators/rosgazebo/include/RosGazeboNode/RosGazeboNode.h 2008-07-30 05:46:12 UTC (rev 2302)
@@ -79,6 +79,8 @@
std_msgs::RobotBase2DOdom odomMsg;
rostools::Time timeMsg;
+ std_msgs::Point3DFloat32 objectPosMsg;
+
// A mutex to lock access to fields that are used in message callbacks
ros::thread::mutex lock;
Modified: pkg/trunk/simulators/rosgazebo/src/RosGazeboNode.cpp
===================================================================
--- pkg/trunk/simulators/rosgazebo/src/RosGazeboNode.cpp 2008-07-30 02:27:50 UTC (rev 2301)
+++ pkg/trunk/simulators/rosgazebo/src/RosGazeboNode.cpp 2008-07-30 05:46:12 UTC (rev 2302)
@@ -47,7 +47,9 @@
// this->PR2Copy->SetArmJointPosition(PR2::PR2_LEFT_ARM, jointPosition, jointSpeed);
*/
+ printf("boo!\n");
if(!useControllerArray){
+ printf("hoo!\n");
this->PR2Copy->hw.SetJointServoCmd(PR2::ARM_R_PAN , this->rightarm.turretAngle, 0);
this->PR2Copy->hw.SetJointServoCmd(PR2::ARM_R_SHOULDER_PITCH, this->rightarm.shoulderLiftAngle, 0);
@@ -345,6 +347,7 @@
advertise<std_msgs::PR2Arm>("right_pr2arm_pos");
advertise<rostools::Time>("time");
advertise<std_msgs::Empty>("transform");
+ advertise<std_msgs::Point3DFloat32>("object_position");
subscribe("cmd_vel", velMsg, &RosGazeboNode::cmdvelReceived);
subscribe("cmd_leftarmconfig", leftarm, &RosGazeboNode::cmd_leftarmconfigReceived);
@@ -625,6 +628,17 @@
// those values are reused in the sendInverseEuler() call above.
publish("odom",this->odomMsg);
+ /***************************************************************/
+ /* */
+ /* object position */
+ /* */
+ /***************************************************************/
+ this->PR2Copy->GetObjectPositionActual(&x,&y,&z,&roll,&pitch,&yaw);
+ this->objectPosMsg.x = x;
+ this->objectPosMsg.y = y;
+ this->objectPosMsg.z = z;
+ publish("object_position", this->objectPosMsg);
+
/***************************************************************/
/* */
/* camera */
Modified: pkg/trunk/util/kinematics/libKDL/src/kdl_kinematics.cpp
===================================================================
--- pkg/trunk/util/kinematics/libKDL/src/kdl_kinematics.cpp 2008-07-30 02:27:50 UTC (rev 2301)
+++ pkg/trunk/util/kinematics/libKDL/src/kdl_kinematics.cpp 2008-07-30 05:46:12 UTC (rev 2302)
@@ -100,9 +100,6 @@
if (this->ik_pos_solver->CartToJnt(*this->q_IK_guess,f,*this->q_IK_result) >= 0)
{
angle_within_mod180(*this->q_IK_result, this->nJnts);
- cout<<"IK guess:"<<*this->q_IK_guess<<endl;
- cout<<"IK result:"<<*this->q_IK_result<<endl;
-
return true;
}
else
Modified: pkg/trunk/util/kinematics/libKDL/test/pr2_kin_test.cpp
===================================================================
--- pkg/trunk/util/kinematics/libKDL/test/pr2_kin_test.cpp 2008-07-30 02:27:50 UTC (rev 2301)
+++ pkg/trunk/util/kinematics/libKDL/test/pr2_kin_test.cpp 2008-07-30 05:46:12 UTC (rev 2302)
@@ -17,11 +17,18 @@
struct timeval t0, t1;
JntArray pr2_config = JntArray(pr2_kin.nJnts);
+ pr2_config(0) = 0.0, pr2_config(1) = -0, pr2_config(2)=0.0, pr2_config(3)=0.0;
+ pr2_config(4) = 0.0, pr2_config(5) = deg2rad*0, pr2_config(6) = deg2rad*90.0;
+ cout<<"Config of the arm:"<<pr2_config<<endl;
+
+ Frame f;
+ if (pr2_kin.FK(pr2_config,f))
+ cout<<"End effector transformation:"<<f<<endl;
+
pr2_config(0) = 0.1, pr2_config(1) = -1, pr2_config(2)=0.3, pr2_config(3)=0.3;
pr2_config(4) = 0.2, pr2_config(5) = 0.5, pr2_config(6) = 0.0;
cout<<"Config of the arm:"<<pr2_config<<endl;
- Frame f;
if (pr2_kin.FK(pr2_config,f))
cout<<"End effector transformation:"<<f<<endl;
else
Modified: pkg/trunk/vision/cam_viewer/cam_viewer.cpp
===================================================================
--- pkg/trunk/vision/cam_viewer/cam_viewer.cpp 2008-07-30 02:27:50 UTC (rev 2301)
+++ pkg/trunk/vision/cam_viewer/cam_viewer.cpp 2008-07-30 05:46:12 UTC (rev 2302)
@@ -12,18 +12,29 @@
{
public:
std_msgs::Image image_msg;
+ char key;
+ int imgnum;
CvView() : node("cam_viewer")
{
cvNamedWindow("cam_viewer", CV_WINDOW_AUTOSIZE);
subscribe("image", image_msg, &CvView::image_cb);
+ imgnum=0;
}
void image_cb()
{
IplImage *cv_image = cvCreateImage( cvSize( image_msg.width, image_msg.height), IPL_DEPTH_8U, 3);
memcpy(cv_image->imageData, image_msg.data, cv_image->imageSize);
cvShowImage("cam_viewer", cv_image);
- cvWaitKey(10);
+ key = cvWaitKey(10);
+ if (key==' ')
+ {
+ char imgname[30];
+ sprintf(imgname, "img_%d.png", imgnum);
+ cvSaveImage(imgname, cv_image);
+ imgnum++;
+ }
+
cvReleaseImage(&cv_image);
}
};
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <rob...@us...> - 2008-07-30 21:53:21
|
Revision: 2340
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=2340&view=rev
Author: rob_wheeler
Date: 2008-07-30 21:53:27 +0000 (Wed, 30 Jul 2008)
Log Message:
-----------
Move tinyxml/include/tinyxml-2.5.3 to tinyxml/include/tinyxml
Modified Paths:
--------------
pkg/trunk/mechanism/mechanism_control/include/mechanism_control/mechanism_control.h
pkg/trunk/robot_descriptions/wg_robot_description_parser/include/urdf/URDF.h
pkg/trunk/robot_descriptions/wg_robot_description_parser/src/urdf/merge.cpp
Modified: pkg/trunk/mechanism/mechanism_control/include/mechanism_control/mechanism_control.h
===================================================================
--- pkg/trunk/mechanism/mechanism_control/include/mechanism_control/mechanism_control.h 2008-07-30 21:50:49 UTC (rev 2339)
+++ pkg/trunk/mechanism/mechanism_control/include/mechanism_control/mechanism_control.h 2008-07-30 21:53:27 UTC (rev 2340)
@@ -31,7 +31,7 @@
#include <map>
#include <string>
#include <vector>
-#include "tinyxml-2.5.3/tinyxml.h"
+#include "tinyxml/tinyxml.h"
#include <hw_interface/hardware_interface.h>
#include <mechanism_model/robot.h>
#include <rosthread/mutex.h>
Modified: pkg/trunk/robot_descriptions/wg_robot_description_parser/include/urdf/URDF.h
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description_parser/include/urdf/URDF.h 2008-07-30 21:50:49 UTC (rev 2339)
+++ pkg/trunk/robot_descriptions/wg_robot_description_parser/include/urdf/URDF.h 2008-07-30 21:53:27 UTC (rev 2340)
@@ -35,7 +35,7 @@
#ifndef URDF_PARSER_
#define URDF_PARSER_
-#include <tinyxml-2.5.3/tinyxml.h>
+#include <tinyxml/tinyxml.h>
#include <istream>
#include <string>
#include <vector>
Modified: pkg/trunk/robot_descriptions/wg_robot_description_parser/src/urdf/merge.cpp
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description_parser/src/urdf/merge.cpp 2008-07-30 21:50:49 UTC (rev 2339)
+++ pkg/trunk/robot_descriptions/wg_robot_description_parser/src/urdf/merge.cpp 2008-07-30 21:53:27 UTC (rev 2340)
@@ -32,7 +32,7 @@
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
-#include <tinyxml-2.5.3/tinyxml.h>
+#include <tinyxml/tinyxml.h>
#include <vector>
#include <string>
#include <fstream>
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <is...@us...> - 2008-07-30 22:13:09
|
Revision: 2342
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=2342&view=rev
Author: isucan
Date: 2008-07-30 22:13:15 +0000 (Wed, 30 Jul 2008)
Log Message:
-----------
use new group information in kinematic planning
Modified Paths:
--------------
pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp
pkg/trunk/motion_planning/plan_kinematic_path/src/plan_kinematic_path.cpp
pkg/trunk/util/collision_space/include/collision_space/environment.h
pkg/trunk/util/collision_space/src/collision_space/environment.cpp
pkg/trunk/util/test_collision_space/src/test_kinematic_ode.cpp
Modified: pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp 2008-07-30 21:55:02 UTC (rev 2341)
+++ pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp 2008-07-30 22:13:15 UTC (rev 2342)
@@ -299,44 +299,49 @@
void addRobotDescriptionFromFile(const char *filename)
{
- robot_desc::URDF *file = new robot_desc::URDF(filename);
- addRobotDescription(file);
+ robot_desc::URDF *file = new robot_desc::URDF();
+ if (file->loadFile(filename))
+ addRobotDescription(file);
+ else
+ delete file;
}
void addRobotDescriptionFromData(const char *data)
{
robot_desc::URDF *file = new robot_desc::URDF();
- file->loadString(data);
- addRobotDescription(file);
+ if (file->loadString(data))
+ addRobotDescription(file);
+ else
+ delete file;
}
void addRobotDescription(robot_desc::URDF *file)
{
m_robotDescriptions.push_back(file);
- std::vector<std::string> groups;
- file->getGroupNames(groups);
- std::string name = file->getRobotName();
-
/* create a model for the whole robot (with the name given in the file) */
Model *model = new Model();
- unsigned int cid = m_collisionSpace->addRobotModel(*file);
+ planning_models::KinematicModel *kmodel = new planning_models::KinematicModel();
+ kmodel->build(*file);
+ unsigned int cid = m_collisionSpace->addRobotModel(kmodel);
model->collisionSpaceID = cid;
model->collisionSpace = m_collisionSpace;
model->kmodel = m_collisionSpace->getModel(cid);
m_models[name] = model;
createMotionPlanningInstances(model);
+ std::vector<std::string> groups;
+ kmodel->getGroups(groups);
+
/* create a model for each group */
for (unsigned int i = 0 ; i < groups.size() ; ++i)
{
Model *model = new Model();
- std::string gname = name + "::" + groups[i];
model->collisionSpaceID = cid;
model->collisionSpace = m_collisionSpace;
model->kmodel = m_collisionSpace->getModel(cid);
- model->groupID = model->kmodel->getGroupID(gname);
- m_models[gname] = model;
+ model->groupID = model->kmodel->getGroupID(groups[i]);
+ m_models[groups[i]] = model;
createMotionPlanningInstances(model);
}
}
Modified: pkg/trunk/motion_planning/plan_kinematic_path/src/plan_kinematic_path.cpp
===================================================================
--- pkg/trunk/motion_planning/plan_kinematic_path/src/plan_kinematic_path.cpp 2008-07-30 21:55:02 UTC (rev 2341)
+++ pkg/trunk/motion_planning/plan_kinematic_path/src/plan_kinematic_path.cpp 2008-07-30 22:13:15 UTC (rev 2342)
@@ -48,7 +48,7 @@
robot_srvs::KinematicMotionPlan::request req;
robot_srvs::KinematicMotionPlan::response res;
- req.model_id = "pr2::leftArm";
+ req.model_id = "pr2::base+rightArm";
req.start_state.set_vals_size(32);
for (unsigned int i = 0 ; i < req.start_state.vals_size ; ++i)
@@ -57,11 +57,11 @@
for (unsigned int i = 18 ; i < 25 ; ++i)
req.start_state.vals[i] = 0.1;
- req.goal_state.set_vals_size(7);
+ req.goal_state.set_vals_size(11);
for (unsigned int i = 0 ; i < req.goal_state.vals_size ; ++i)
req.goal_state.vals[i] = -0.4;
- req.allowed_time = 2.0;
+ req.allowed_time = 15.0;
req.volumeMin.x = -1.0; req.volumeMin.y = -1.0; req.volumeMin.z = -1.0;
req.volumeMax.x = -1.0; req.volumeMax.y = -1.0; req.volumeMax.z = -1.0;
Modified: pkg/trunk/util/collision_space/include/collision_space/environment.h
===================================================================
--- pkg/trunk/util/collision_space/include/collision_space/environment.h 2008-07-30 21:55:02 UTC (rev 2341)
+++ pkg/trunk/util/collision_space/include/collision_space/environment.h 2008-07-30 22:13:15 UTC (rev 2342)
@@ -71,8 +71,6 @@
virtual void addPointCloud(unsigned int n, const double* points, double radius = 0.01) = 0;
/** Add a robot model */
- virtual unsigned int addRobotModel(robot_desc::URDF &pmodel, const char *group = NULL);
- /** Add a robot model */
virtual unsigned int addRobotModel(planning_models::KinematicModel *model);
/** Update the positions of the geometry used in collision detection */
Modified: pkg/trunk/util/collision_space/src/collision_space/environment.cpp
===================================================================
--- pkg/trunk/util/collision_space/src/collision_space/environment.cpp 2008-07-30 21:55:02 UTC (rev 2341)
+++ pkg/trunk/util/collision_space/src/collision_space/environment.cpp 2008-07-30 22:13:15 UTC (rev 2342)
@@ -34,13 +34,6 @@
#include <collision_space/environment.h>
-unsigned int collision_space::EnvironmentModel::addRobotModel(robot_desc::URDF &pmodel, const char *group)
-{
- planning_models::KinematicModel *m = new planning_models::KinematicModel();
- m->build(pmodel, group);
- return addRobotModel(m);
-}
-
unsigned int collision_space::EnvironmentModel::addRobotModel(planning_models::KinematicModel *model)
{
unsigned int pos = m_models.size();
Modified: pkg/trunk/util/test_collision_space/src/test_kinematic_ode.cpp
===================================================================
--- pkg/trunk/util/test_collision_space/src/test_kinematic_ode.cpp 2008-07-30 21:55:02 UTC (rev 2341)
+++ pkg/trunk/util/test_collision_space/src/test_kinematic_ode.cpp 2008-07-30 22:13:15 UTC (rev 2342)
@@ -100,8 +100,9 @@
model.loadFile("/u/isucan/ros/ros-pkg/robot_descriptions/wg_robot_description/pr2/pr2.xml");
EnvironmentModel *km = new EnvironmentModelODE();
- km->addRobotModel(model);
- planning_models::KinematicModel *m = km->getModel(0);
+ planning_models::KinematicModel *m = new planning_models::KinematicModel();
+ m->build(model);
+ km->addRobotModel(m);
printModelInfo(m);
double *param = new double[m->stateDimension];
@@ -113,8 +114,8 @@
for (unsigned int i = 0 ; i < m->stateDimension ; ++i)
- param[i] = -0.3;
- m->computeTransforms(param, m->getGroupID("pr2::leftArm"));
+ param[i] = +0.3;
+ m->computeTransforms(param, m->getGroupID("pr2::base+rightArm"));
km->updateRobotModel(0);
EnvironmentModelODE* okm = dynamic_cast<EnvironmentModelODE*>(km);
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
Thanks for helping keep SourceForge clean.
X