Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[8598] pkg/trunk
|
From: <rdi...@us...> - 2008-12-26 22:22:25
|
Revision: 8598
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=8598&view=rev
Author: rdiankov
Date: 2008-12-26 21:50:08 +0000 (Fri, 26 Dec 2008)
Log Message:
-----------
added an accurate gripper transmission model, added an openraveros robot controller interface
Modified Paths:
--------------
pkg/trunk/3rdparty/openrave/Makefile
pkg/trunk/mechanism/mechanism_model/include/mechanism_model/gripper_transmission.h
pkg/trunk/mechanism/mechanism_model/src/gripper_transmission.cpp
pkg/trunk/openrave_planning/ormanipulation/data/pr2table_real.env.xml
pkg/trunk/openrave_planning/ormanipulation/octave/SetupTableScene.m
pkg/trunk/openrave_planning/ormanipulation/octave/runperception.m
pkg/trunk/openrave_planning/orrosplanning/manifest.xml
pkg/trunk/openrave_planning/orrosplanning/src/objecttransformsystem.h
pkg/trunk/openrave_planning/orrosplanning/src/orrosplanningmain.cpp
pkg/trunk/openrave_planning/orrosplanning/src/phasespacesystem.h
pkg/trunk/openrave_planning/orrosplanning/src/plugindefs.h
pkg/trunk/openrave_planning/orrosplanning/src/rossensorsystem.h
pkg/trunk/openrave_planning/orrosplanning/src/simplesensorsystem.h
pkg/trunk/openrave_planning/setopenrave.sh
pkg/trunk/robot_descriptions/wg_robot_description/pr2_robot_defs/gripper_defs.xml
Added Paths:
-----------
pkg/trunk/openrave_planning/orrosplanning/src/rosrobotcontroller.h
Modified: pkg/trunk/3rdparty/openrave/Makefile
===================================================================
--- pkg/trunk/3rdparty/openrave/Makefile 2008-12-26 04:12:22 UTC (rev 8597)
+++ pkg/trunk/3rdparty/openrave/Makefile 2008-12-26 21:50:08 UTC (rev 8598)
@@ -2,7 +2,7 @@
SVN_DIR = openrave_svn
# Should really specify a revision
-SVN_REVISION = -r 587
+SVN_REVISION = -r 588
SVN_URL = https://openrave.svn.sourceforge.net/svnroot/openrave
include $(shell rospack find mk)/svn_checkout.mk
Modified: pkg/trunk/mechanism/mechanism_model/include/mechanism_model/gripper_transmission.h
===================================================================
--- pkg/trunk/mechanism/mechanism_model/include/mechanism_model/gripper_transmission.h 2008-12-26 04:12:22 UTC (rev 8597)
+++ pkg/trunk/mechanism/mechanism_model/include/mechanism_model/gripper_transmission.h 2008-12-26 21:50:08 UTC (rev 8598)
@@ -55,7 +55,7 @@
class GripperTransmission : public Transmission
{
public:
- GripperTransmission() {}
+ GripperTransmission() : A_(1), B_(0) {}
virtual ~GripperTransmission() {}
bool initXml(TiXmlElement *config, Robot *robot);
@@ -67,6 +67,9 @@
std::vector<double> reductions_; // Mechanical reduction for each joint
std::vector<control_toolbox::Pid> pids_; // For keeping the joint angles aligned in Gazebo
+
+private:
+ double A_, B_; // gripper angle = reduction*acos(A*motor+B)
};
} // namespace mechanism
Modified: pkg/trunk/mechanism/mechanism_model/src/gripper_transmission.cpp
===================================================================
--- pkg/trunk/mechanism/mechanism_model/src/gripper_transmission.cpp 2008-12-26 04:12:22 UTC (rev 8597)
+++ pkg/trunk/mechanism/mechanism_model/src/gripper_transmission.cpp 2008-12-26 21:50:08 UTC (rev 8598)
@@ -44,6 +44,13 @@
const char *name = config->Attribute("name");
name_ = name ? name : "";
+ const char *pA = config->Attribute("A");
+ if( pA != NULL )
+ A_ = atof(pA);
+ const char *pB = config->Attribute("B");
+ if( pB != NULL )
+ B_ = atof(pB);
+
TiXmlElement *ael = config->FirstChildElement("actuator");
const char *actuator_name = ael ? ael->Attribute("name") : NULL;
if (!actuator_name || !robot->getActuator(actuator_name))
@@ -91,10 +98,22 @@
{
assert(as.size() == 1);
assert(js.size() == reductions_.size());
+
+ // cos(pos*reduction) = A*motor+B
+ // -reduction * sin(pos*reduction) * dpos/dt = A * dmotor/dt
+ double cang = as[0]->state_.position_*A_+B_;
+ double ang;
+ if( cang < -1 )
+ ang = M_PI;
+ else if( cang > 1 )
+ ang = 0;
+ else
+ ang = acos(cang);
+
for (unsigned int i = 0; i < js.size(); ++i)
{
- js[i]->position_ = as[0]->state_.position_ / reductions_[i];
- js[i]->velocity_ = as[0]->state_.velocity_ / reductions_[i];
+ js[i]->position_ = ang / reductions_[i];
+ js[i]->velocity_ = - A_*as[0]->state_.velocity_ / (sin(ang)*reductions_[i]);
js[i]->applied_effort_ = as[0]->state_.last_measured_effort_ * reductions_[i];
}
}
@@ -110,8 +129,8 @@
double mean_effort = 0.0;
for (unsigned int i = 0; i < js.size(); ++i)
{
- mean_position += js[i]->position_ * reductions_[i];
- mean_velocity += js[i]->velocity_ * reductions_[i];
+ mean_position += (cos(js[i]->position_ * reductions_[i])-B_)/A_;
+ mean_velocity += - js[i]->velocity_ * reductions_[i] * sin(js[i]->position_*reductions_[i]) / A_;
mean_effort += js[i]->applied_effort_ / (reductions_[i]);
}
as[0]->state_.position_ = mean_position / js.size();
Modified: pkg/trunk/openrave_planning/ormanipulation/data/pr2table_real.env.xml
===================================================================
--- pkg/trunk/openrave_planning/ormanipulation/data/pr2table_real.env.xml 2008-12-26 04:12:22 UTC (rev 8597)
+++ pkg/trunk/openrave_planning/ormanipulation/data/pr2table_real.env.xml 2008-12-26 21:50:08 UTC (rev 8598)
@@ -1,10 +1,7 @@
<Environment>
- <bkgndcolor>1 1 1</bkgndcolor>
- <camtrans> -0.285786 -4.231486 0.296317</camtrans>
- <camrotaxis>-0.998283 0.046406 0.035754 265</camrotaxis>
-
-<!-- <camtrans>0.792115 1.544696 2.088982</camtrans>
- <camrotaxis>-0.092050 -0.490650 -0.866481 211</camrotaxis> -->
+ <bkgndcol>0.3 0.7 0.8</bkgndcol>
+ <camtrans>0.792115 1.544696 2.088982</camtrans>
+ <camrotaxis>-0.092050 -0.490650 -0.866481 211</camrotaxis>
<Robot file="robots/pr2full.robot.xml">
<translation>-0.708 0.005 0.0</translation>
Modified: pkg/trunk/openrave_planning/ormanipulation/octave/SetupTableScene.m
===================================================================
--- pkg/trunk/openrave_planning/ormanipulation/octave/SetupTableScene.m 2008-12-26 04:12:22 UTC (rev 8597)
+++ pkg/trunk/openrave_planning/ormanipulation/octave/SetupTableScene.m 2008-12-26 21:50:08 UTC (rev 8598)
@@ -56,7 +56,7 @@
orEnvLoadScene('', 1);
%% create rosproblem before everything so resources can init!
-probs.rosplan = orEnvCreateProblem('ROSPlanningProblem');
+probs.rosplan = orEnvCreateProblem('ROSPlanning');
if( isempty(probs.rosplan) )
error('failed to create problem');
end
Modified: pkg/trunk/openrave_planning/ormanipulation/octave/runperception.m
===================================================================
--- pkg/trunk/openrave_planning/ormanipulation/octave/runperception.m 2008-12-26 04:12:22 UTC (rev 8597)
+++ pkg/trunk/openrave_planning/ormanipulation/octave/runperception.m 2008-12-26 21:50:08 UTC (rev 8598)
@@ -4,9 +4,10 @@
cd(fullfile(rosoct_findpackage('ormanipulation'),'octave'));
startup;
-orEnvSetOptions('debug debug');
+%orEnvSetOptions('debug debug');
robot = SetupTableScene('data/pr2table_real.env.xml');
+orRobotControllerSet(robot.id, 'ROSRobot');
out = orProblemSendCommand('createsystem ObjectTransform /checkerdetector/ObjectDetection 0.1',probs.task);
if( isempty(out) )
Modified: pkg/trunk/openrave_planning/orrosplanning/manifest.xml
===================================================================
--- pkg/trunk/openrave_planning/orrosplanning/manifest.xml 2008-12-26 04:12:22 UTC (rev 8597)
+++ pkg/trunk/openrave_planning/orrosplanning/manifest.xml 2008-12-26 21:50:08 UTC (rev 8598)
@@ -11,4 +11,5 @@
<depend package="libKinematics"/>
<depend package="boost"/>
<depend package="checkerboard_detector"/>
+ <depend package="robot_msgs"/>
</package>
Modified: pkg/trunk/openrave_planning/orrosplanning/src/objecttransformsystem.h
===================================================================
--- pkg/trunk/openrave_planning/orrosplanning/src/objecttransformsystem.h 2008-12-26 04:12:22 UTC (rev 8597)
+++ pkg/trunk/openrave_planning/orrosplanning/src/objecttransformsystem.h 2008-12-26 21:50:08 UTC (rev 8598)
@@ -29,8 +29,6 @@
#include "rossensorsystem.h"
#include "checkerboard_detector/ObjectDetection.h"
-using namespace ros;
-
// used to update objects through a mocap system
class ObjectTransformXMLID
{
Modified: pkg/trunk/openrave_planning/orrosplanning/src/orrosplanningmain.cpp
===================================================================
--- pkg/trunk/openrave_planning/orrosplanning/src/orrosplanningmain.cpp 2008-12-26 04:12:22 UTC (rev 8597)
+++ pkg/trunk/openrave_planning/orrosplanning/src/orrosplanningmain.cpp 2008-12-26 21:50:08 UTC (rev 8598)
@@ -28,7 +28,7 @@
#include "rosarmik.h"
#include "phasespacesystem.h"
#include "objecttransformsystem.h"
-
+#include "rosrobotcontroller.h"
#include "rosplanningproblem.h"
// declaring variables with stdcall can be a little complex
@@ -53,18 +53,24 @@
InterfaceBase* DECL_STDCALL(ORCreate, (PluginType type, wchar_t* name, EnvironmentBase* penv))
{
switch(type) {
+ case PT_Controller:
+ if( wcsicmp(name, L"ROSRobot") == 0 )
+ return new ROSRobotController(penv);
+ break;
case PT_InverseKinematicsSolver:
if( wcsicmp(name, L"ROSArmIK") == 0 )
return new ROSArmIK(penv);
break;
case PT_ProblemInstance:
- if( wcsicmp(name, L"ROSPlanningProblem") == 0 )
+ if( wcsicmp(name, L"ROSPlanning") == 0 )
return new ROSPlanningProblem(penv);
+ break;
case PT_SensorSystem:
if( wcsicmp(name, L"PhaseSpace") == 0 )
return new PhaseSpaceMocapClient(penv);
if( wcsicmp(name, L"ObjectTransform") == 0 )
return new ObjectTransformSystem(penv);
+ break;
default:
break;
}
@@ -83,7 +89,7 @@
pinfo->iksolvers.push_back(L"ROSArmIK");
pinfo->sensorsystems.push_back(L"PhaseSpace");
pinfo->sensorsystems.push_back(L"ObjectTransform");
- pinfo->problems.push_back(L"ROSPlanningProblem");
+ pinfo->problems.push_back(L"ROSPlanning");
return true;
}
Modified: pkg/trunk/openrave_planning/orrosplanning/src/phasespacesystem.h
===================================================================
--- pkg/trunk/openrave_planning/orrosplanning/src/phasespacesystem.h 2008-12-26 04:12:22 UTC (rev 8597)
+++ pkg/trunk/openrave_planning/orrosplanning/src/phasespacesystem.h 2008-12-26 21:50:08 UTC (rev 8598)
@@ -29,8 +29,6 @@
#include "rossensorsystem.h"
#include "phase_space/PhaseSpaceSnapshot.h"
-using namespace ros;
-
// used to update objects through a mocap system
class PhaseSpaceXMLID
{
Modified: pkg/trunk/openrave_planning/orrosplanning/src/plugindefs.h
===================================================================
--- pkg/trunk/openrave_planning/orrosplanning/src/plugindefs.h 2008-12-26 04:12:22 UTC (rev 8597)
+++ pkg/trunk/openrave_planning/orrosplanning/src/plugindefs.h 2008-12-26 21:50:08 UTC (rev 8598)
@@ -143,7 +143,7 @@
#endif
-inline std::wstring _ravembstowcs(const char* pstr)
+inline std::wstring _stdmbstowcs(const char* pstr)
{
size_t len = mbstowcs(NULL, pstr, 0);
std::wstring w; w.resize(len);
@@ -151,6 +151,18 @@
return w;
}
+inline string _stdwcstombs(const wchar_t* pname)
+{
+ string s;
+ size_t len = wcstombs(NULL, pname, 0);
+ if( len != (size_t)-1 ) {
+ s.resize(len);
+ wcstombs(&s[0], pname, len);
+ }
+
+ return s;
+}
+
#include <rave/rave.h>
using namespace OpenRAVE;
@@ -160,6 +172,8 @@
#include <boost/shared_ptr.hpp>
#include <boost/thread/mutex.hpp>
+using namespace ros;
+
inline ros::node* check_roscpp()
{
// start roscpp
Added: pkg/trunk/openrave_planning/orrosplanning/src/rosrobotcontroller.h
===================================================================
--- pkg/trunk/openrave_planning/orrosplanning/src/rosrobotcontroller.h (rev 0)
+++ pkg/trunk/openrave_planning/orrosplanning/src/rosrobotcontroller.h 2008-12-26 21:50:08 UTC (rev 8598)
@@ -0,0 +1,265 @@
+// Software License Agreement (BSD License)
+// Copyright (c) 2008, Willow Garage, Inc.
+// 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.
+// * The name of the author may not 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.
+//
+// author: Rosen Diankov
+#ifndef RAVE_ROS_ROBOT_CONTROLLER
+#define RAVE_ROS_ROBOT_CONTROLLER
+
+#include <robot_msgs/MechanismState.h>
+
+// controller for the SSC-32 board
+class ROSRobotController : public ControllerBase
+{
+ enum ControllerState{
+ None = 0,
+ Servo, // done when servoed to position and the position is held
+ Traj, // done when reaches last point
+ Velocity // done when joints stop moving
+ };
+
+public:
+ ROSRobotController(EnvironmentBase* penv) : ControllerBase(penv), _topic("mechanism_state"), _fTimeCommandStarted(0),
+ _ptraj(NULL), _bIsDone(false), _bSendTimestamps(false), _bSubscribed(false), _bCalibrated(false) {
+ }
+
+ virtual ~ROSRobotController() {
+ Destroy();
+ }
+
+ /// args format: host port [proxytype index]
+ /// where proxytype is actarray, pos2d, or ...
+ /// the order specified is the order the degrees of freedom will be arranged
+ virtual bool Init(RobotBase* robot, const char* args)
+ {
+ Destroy();
+ _probot = robot;
+ if( _probot == NULL )
+ return false;
+
+ startsubscriptions();
+
+ return _bSubscribed;
+ }
+
+ virtual void Destroy()
+ {
+ stopsubscriptions();
+ _bCalibrated = false;
+ _probot = NULL;
+ _bIsDone = false;
+ }
+
+ virtual void Reset(int options)
+ {
+ }
+
+ virtual bool SetDesired(const dReal* pValues)
+ {
+ return true;
+ }
+
+ virtual bool SetPath(const Trajectory* ptraj)
+ {
+ return true;
+ }
+
+ virtual bool SetPath(const Trajectory* ptraj, int nTrajectoryId, float fDivergenceTime)
+ {
+ RAVELOG_ERRORA("ros controller does not support path with divergence");
+ return false;
+ }
+
+ virtual int GetDOF() { return _probot != NULL ? _probot->GetDOF() : 0; }
+
+ virtual bool SimulationStep(float fTimeElapsed)
+ {
+ if( !_bSubscribed )
+ return false;
+
+ {
+ boost::mutex::scoped_lock lock(_mutexstate);
+ if( _bCalibrated ) {
+ vector<dReal> values;
+ values.reserve(_mstate.get_joint_states_size());
+ FOREACHC(itj, _vjointmap)
+ values.push_back(_mstate.joint_states[*itj].position);
+
+ ROS_ASSERT( (int)values.size() == _probot->GetDOF() );
+ _probot->SetJointValues(NULL, NULL, &values[0], true);
+ }
+ }
+
+ return IsDone();
+ }
+
+ virtual bool SendCmd(const char* pcmd)
+ {
+ return false;
+ }
+
+ virtual bool SupportsCmd(const char* pcmd)
+ {
+ return false;
+ }
+
+ virtual bool IsDone() { return _bIsDone; }
+
+ virtual ActuatorState GetActuatorState(int index)
+ {
+ return AS_Idle;
+ }
+
+ virtual float GetTime() const
+ {
+ boost::mutex::scoped_lock lock(_mutexstate);
+ return _mstate.time - _fTimeCommandStarted;
+ }
+ virtual RobotBase* GetRobot() const { return _probot; }
+
+ virtual void GetVelocity(std::vector<dReal>& vel) const
+ {
+ vel.resize(0);
+ if( !_bCalibrated )
+ return;
+
+ boost::mutex::scoped_lock lock(_mutexstate);
+ assert( (int)_vjointmap.size() == _probot->GetDOF() );
+
+ vel.reserve(_mstate.get_joint_states_size());
+ FOREACHC(itj, _vjointmap)
+ vel.push_back(_mstate.joint_states[*itj].velocity);
+ }
+
+ virtual void GetTorque(std::vector<dReal>& torque) const
+ {
+ torque.resize(0);
+ if( !_bCalibrated )
+ return;
+
+ boost::mutex::scoped_lock lock(_mutexstate);
+ assert( (int)_vjointmap.size() == _probot->GetDOF() );
+
+ torque.reserve(_mstate.get_joint_states_size());
+ FOREACHC(itj, _vjointmap)
+ torque.push_back(_mstate.joint_states[*itj].applied_effort); // commanded_effort?
+ }
+
+private:
+
+ virtual void startsubscriptions()
+ {
+ // check if thread launched
+ _bSubscribed = false;
+ ros::node* pnode = check_roscpp();
+ if( pnode != NULL ) {
+ _bSubscribed = pnode->subscribe(_topic, _mstate_cb, &ROSRobotController::mechanismstatecb, this, 10);
+ if( _bSubscribed )
+ RAVELOG_DEBUGA("subscribed to %s\n", _topic.c_str());
+ else
+ RAVELOG_ERRORA("failed to subscribe to %s\n", _topic.c_str());
+ }
+ }
+
+ virtual void stopsubscriptions()
+ {
+ if( _bSubscribed ) {
+ ros::node* pnode = check_roscpp_nocreate();
+ if( pnode != NULL ) {
+ pnode->unsubscribe(_topic.c_str());
+ RAVELOG_DEBUGA("unsubscribe from %s\n", _topic.c_str());
+ }
+ _bSubscribed = false;
+ }
+ }
+
+ virtual void mechanismstatecb()
+ {
+ if( !_bCalibrated ) {
+ // check the robot joint/link names
+ GetEnv()->LockPhysics(true);
+
+ do {
+ _vjointmap.resize(0);
+ for(int i = 0; i < _probot->GetDOF(); ++i) {
+ for(size_t j = 0; j < _mstate_cb.get_joint_states_size(); ++j) {
+ if( _stdwcstombs(_probot->GetJoints()[i]->GetName()) == _mstate_cb.joint_states[j].name ) {
+ _vjointmap.push_back(j);
+ break;
+ }
+ }
+
+ if( i+1 != (int)_vjointmap.size()) {
+ RAVELOG_WARNA("could not find robot joint %S in mechanism state\n", _probot->GetJoints()[i]->GetName());
+ break;
+ }
+ }
+
+ if( _probot->GetDOF() != (int)_vjointmap.size() ) {
+ _vjointmap.resize(0);
+ break;
+ }
+
+ _bCalibrated = true;
+ } while(0);
+
+ GetEnv()->LockPhysics(false);
+ }
+ else {
+ if( _probot->GetDOF() != (int)_mstate_cb.get_joint_states_size() )
+ _bCalibrated = false;
+ }
+
+ if( !_bCalibrated )
+ return;
+
+ {
+ boost::mutex::scoped_lock lock(_mutexstate);
+ _mstate = _mstate_cb;
+ }
+
+ // do some monitoring of the joint state (try to look for stalls)
+ }
+
+ RobotBase* _probot; ///< robot owning this controller
+
+ string _topic;
+ robot_msgs::MechanismState _mstate_cb, _mstate;
+ vector<dReal> _vecdesired;
+ mutable boost::mutex _mutexstate;
+
+ ofstream flog;
+ int logid;
+
+ vector<int> _vjointmap; ///< maps mechanism state joints to robot joints
+
+ double _fTimeCommandStarted;
+ const Trajectory* _ptraj;
+
+ bool _bIsDone;
+ bool _bSendTimestamps; ///< if true, will send timestamps along with traj
+ bool _bSubscribed; ///< if true, succesfully subscribed to the mechanism state msgs
+ bool _bCalibrated; ///< if true, mechanism state matches robot
+};
+
+#endif
Modified: pkg/trunk/openrave_planning/orrosplanning/src/rossensorsystem.h
===================================================================
--- pkg/trunk/openrave_planning/orrosplanning/src/rossensorsystem.h 2008-12-26 04:12:22 UTC (rev 8597)
+++ pkg/trunk/openrave_planning/orrosplanning/src/rossensorsystem.h 2008-12-26 21:50:08 UTC (rev 8598)
@@ -28,8 +28,6 @@
#include "simplesensorsystem.h"
-using namespace ros;
-
// used to update objects through a mocap system
template <typename T, typename XMLID>
class ROSSensorSystem : public SimpleSensorSystem<XMLID>
Modified: pkg/trunk/openrave_planning/orrosplanning/src/simplesensorsystem.h
===================================================================
--- pkg/trunk/openrave_planning/orrosplanning/src/simplesensorsystem.h 2008-12-26 04:12:22 UTC (rev 8597)
+++ pkg/trunk/openrave_planning/orrosplanning/src/simplesensorsystem.h 2008-12-26 21:50:08 UTC (rev 8598)
@@ -94,7 +94,7 @@
if( stricmp((const char*)name, "offsetlink") == 0 ) {
string linkname;
ss >> linkname;
- _pMocap->strOffsetLink = _ravembstowcs(linkname.c_str());
+ _pMocap->strOffsetLink = _stdmbstowcs(linkname.c_str());
}
else if( stricmp((const char*)name, "id") == 0 )
ss >> _pMocap->id;
Modified: pkg/trunk/openrave_planning/setopenrave.sh
===================================================================
--- pkg/trunk/openrave_planning/setopenrave.sh 2008-12-26 04:12:22 UTC (rev 8597)
+++ pkg/trunk/openrave_planning/setopenrave.sh 2008-12-26 21:50:08 UTC (rev 8598)
@@ -2,5 +2,5 @@
# highly recommend to put these commands in the local ~..bashrc file
# otherwise, use source setopenrave.sh
export PATH=`rospack find openrave`/bin:$PATH
-export OPENRAVE_DATA=`rospack find openrave_robot_description`:`openrave-config --prefix`/share/openrave/:$OPENRAVE_DATA
+export OPENRAVE_DATA=`rospack find openrave_robot_description`:`rospack find ormanipulation`:`openrave-config --prefix`/share/openrave/:$OPENRAVE_DATA
export OPENRAVE_PLUGINS=`rospack find orplugins`:`openrave-config --prefix`/share/openrave/plugins:$OPENRAVE_PLUGINS
Modified: pkg/trunk/robot_descriptions/wg_robot_description/pr2_robot_defs/gripper_defs.xml
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description/pr2_robot_defs/gripper_defs.xml 2008-12-26 04:12:22 UTC (rev 8597)
+++ pkg/trunk/robot_descriptions/wg_robot_description/pr2_robot_defs/gripper_defs.xml 2008-12-26 21:50:08 UTC (rev 8598)
@@ -423,20 +423,20 @@
<pr2_finger prefix="${side}_gripper" parent="${side}_gripper_palm" />
- <!--transmission type="GripperTransmission" name="${side}_gripper_trans">
+ <!-- cos(angle*reduction) = motor_encoder * A + B -->
+ <transmission type="GripperTransmission" name="${side}_gripper_trans" A="0.0004268" B="0.72730">
<actuator name="${side}_gripper_motor" />
- <joint name="${side}_gripper_r_finger_joint" reduction="-1000.0" />
- <joint name="${side}_gripper_r_finger_tip_joint" reduction="1000.0" />
- <joint name="${side}_gripper_l_finger_joint" reduction="1000.0" />
- <joint name="${side}_gripper_l_finger_tip_joint" reduction="-1000.0" />
+ <joint name="${side}_gripper_r_finger_joint" reduction="-1.0" />
+ <joint name="${side}_gripper_r_finger_tip_joint" reduction="1.0" />
+ <joint name="${side}_gripper_l_finger_joint" reduction="1.0" />
+ <joint name="${side}_gripper_l_finger_tip_joint" reduction="-1.0" />
<pid p="0.01" i="0.001" d="0.005" iClamp="0.005" />
- </transmission-->
- <transmission type="SimpleTransmission" name="${side}_gripper_trans">
+ </transmission>
+<!-- <transmission type="SimpleTransmission" name="${side}_gripper_trans">
<actuator name="${side}_gripper_motor" />
<joint name="${side}_gripper_l_finger_joint" />
<mechanicalReduction>6.0</mechanicalReduction>
- </transmission>
-
+ </transmission> -->
</macro>
<!-- Calibration -->
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
