Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[19770] pkg/trunk/stacks
|
From: <mp...@us...> - 2009-07-27 23:43:00
|
Revision: 19770
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=19770&view=rev
Author: mppics
Date: 2009-07-27 23:42:46 +0000 (Mon, 27 Jul 2009)
Log Message:
-----------
clean up gripper transmission block (removed pid)
move gripper coefficients screw_reduction and gear_ratio into transmission block.
removed pr2_gripper_alpha2, replace with macro variable for origin (add x-offset of 0.003m) when referring to
pr2_gripper.
update pr2_gripper_transmission:
* to parse the coefficients.
* Jacobian was off by a factor of (2*PI)^2 /2
Modified Paths:
--------------
pkg/trunk/stacks/mechanism/mechanism_model/include/mechanism_model/pr2_gripper_transmission.h
pkg/trunk/stacks/mechanism/mechanism_model/src/pr2_gripper_transmission.cpp
pkg/trunk/stacks/pr2/pr2_defs/defs/gripper_defs.xml
pkg/trunk/stacks/pr2/pr2_defs/robots/l_arm.xacro.xml
pkg/trunk/stacks/pr2/pr2_defs/robots/l_gripper.xacro.xml
pkg/trunk/stacks/pr2/pr2_defs/robots/pr2.xacro.xml
pkg/trunk/stacks/pr2/pr2_defs/robots/pre.xacro.xml
pkg/trunk/stacks/pr2/pr2_defs/robots/prf.xacro.xml
pkg/trunk/stacks/pr2/pr2_defs/robots/prg.xacro.xml
pkg/trunk/stacks/pr2/pr2_defs/robots/r_arm.xacro.xml
pkg/trunk/stacks/pr2/pr2_defs/robots/r_forearm_gripper.xacro.xml
pkg/trunk/stacks/pr2/pr2_defs/robots/r_gripper.xacro.xml
Modified: pkg/trunk/stacks/mechanism/mechanism_model/include/mechanism_model/pr2_gripper_transmission.h
===================================================================
--- pkg/trunk/stacks/mechanism/mechanism_model/include/mechanism_model/pr2_gripper_transmission.h 2009-07-27 23:30:10 UTC (rev 19769)
+++ pkg/trunk/stacks/mechanism/mechanism_model/include/mechanism_model/pr2_gripper_transmission.h 2009-07-27 23:42:46 UTC (rev 19770)
@@ -47,6 +47,7 @@
#include "tinyxml/tinyxml.h"
#include "mechanism_model/transmission.h"
#include "mechanism_model/robot.h"
+//#include <fstream>
namespace mechanism {
@@ -54,7 +55,7 @@
{
public:
PR2GripperTransmission() {}
- virtual ~PR2GripperTransmission() {}
+ virtual ~PR2GripperTransmission() {/*myfile.close();*/}
bool initXml(TiXmlElement *config, Robot *robot);
@@ -78,7 +79,7 @@
void computeGapStates(double MR,double MR_dot,double MT,
double &theta,double &dtheta_dMR,double &dt_dtheta,double &dt_dMR,double &gap_size,double &gap_velocity,double &gap_effort);
void inverseGapStates(double theta,double &MR, double &dMR_dtheta,double &dtheta_dt,double &dMR_dt);
-
+ //std::ofstream myfile;
void getRateFromMaxRateJoint(
std::vector<JointState*>& js, std::vector<Actuator*>& as,
int &maxRateJointIndex,double &rate);
@@ -89,11 +90,11 @@
// SOME CONSTANTS
// the default theta0 when gap size is 0 is needed to assign passive joint angles
//
+ double screw_reduction_; // in meters/revolution
+ double gear_ratio_;
static const double theta0_ = 2.97571*M_PI/180.0; // convert to radians
static const double phi0_ = 29.98717*M_PI/180.0; // convert to radians
- static const double gear_ratio_ = 29.16; //729.0/25.0;
static const double t0_ = 0; //-0.19543/1000.0; // convert to meters
- static const double screw_reduction_ = 2.0/1000.0; // convert to meters
static const double L0_ = 34.70821/1000.0; // convert to meters
static const double coef_h_ = 5.200/1000.0; // convert to meters
static const double coef_a_ = 67.56801/1000.0; // convert to meters
@@ -107,8 +108,6 @@
extern const double PR2GripperTransmission::t0_ ;
extern const double PR2GripperTransmission::theta0_ ;
extern const double PR2GripperTransmission::phi0_ ;
-extern const double PR2GripperTransmission::gear_ratio_ ;
-extern const double PR2GripperTransmission::screw_reduction_ ;
extern const double PR2GripperTransmission::L0_ ;
extern const double PR2GripperTransmission::coef_h_ ;
extern const double PR2GripperTransmission::coef_a_ ;
Modified: pkg/trunk/stacks/mechanism/mechanism_model/src/pr2_gripper_transmission.cpp
===================================================================
--- pkg/trunk/stacks/mechanism/mechanism_model/src/pr2_gripper_transmission.cpp 2009-07-27 23:30:10 UTC (rev 19769)
+++ pkg/trunk/stacks/mechanism/mechanism_model/src/pr2_gripper_transmission.cpp 2009-07-27 23:42:46 UTC (rev 19770)
@@ -36,6 +36,7 @@
#include <numeric>
#include <angles/angles.h>
+
namespace mechanism {
ROS_REGISTER_TRANSMISSION(PR2GripperTransmission)
@@ -44,7 +45,7 @@
{
const char *name = config->Attribute("name");
name_ = name ? name : "";
-
+ //myfile.open("transmission_data.txt");
TiXmlElement *ael = config->FirstChildElement("actuator");
const char *actuator_name = ael ? ael->Attribute("name") : NULL;
if (!actuator_name || !robot->getActuator(actuator_name))
@@ -75,6 +76,25 @@
}
gap_mechanical_reduction_ = atof(joint_reduction);
+ // get the screw drive reduction
+ const char *screw_reduction_str = j->Attribute("screw_reduction");
+ if (screw_reduction_str == NULL)
+ {
+ ROS_ERROR("PR2GripperTransmission's joint \"%s\" was not given a screw drive reduction.", joint_name);
+ return false;
+ }
+ screw_reduction_ = atof(screw_reduction_str);
+ ROS_INFO("screw drive reduction. %f", screw_reduction_);
+
+ // get the gear_ratio reduction
+ const char *gear_ratio_str = j->Attribute("gear_ratio");
+ if (gear_ratio_str == NULL)
+ {
+ ROS_ERROR("PR2GripperTransmission's joint \"%s\" was not given a gear_ratio.", joint_name);
+ return false;
+ }
+ gear_ratio_ = atof(gear_ratio_str);
+ ROS_INFO("gear_ratio reduction. %f", gear_ratio_);
}
for (TiXmlElement *j = config->FirstChildElement("passive_joint"); j; j = j->NextSiblingElement("passive_joint"))
@@ -137,14 +157,12 @@
//
// get the effort at the gripper gap based on torque at the motor
// gap effort = motor torque * dmotor_theta/dt
- // = I * motor_theta_ddot * dmotor_theta_dt
- // = I * MR_ddot * (2*pi) * dmotor_theta_dt
- // = I * MR_ddot * (2*pi) * dMR_dt / (2*pi)
- // = MT / dt_dMR
+ // = MT * dmotor_theta_dt
+ // = MT * dMR_dt / (2*pi)
+ // = MT / dt_dMR * 2*pi
//
- // here MT is defined as I*MR_ddot
- //
- gap_effort = MT / dt_dMR;
+ gap_effort = MT / dt_dMR / rad2mr_ ;
+ //ROS_WARN("debug: %f %f %f",gap_effort,MT,dt_dMR,rad2mr_);
}
///////////////////////////////////////////////////////////
@@ -188,65 +206,6 @@
dMR_dt = dMR_dtheta * dtheta_dt; // remember, here, t is gap_size
}
-///////////////////////////////////////////////////////////
-/// assign joint position, velocity, effort from actuator state
-/// all passive joints are assigned by single actuator state through mimic?
-void PR2GripperTransmission::propagatePosition(
- std::vector<Actuator*>& as, std::vector<JointState*>& js)
-{
-
- ROS_ASSERT(as.size() == 1);
- ROS_ASSERT(js.size() == passive_joints_.size() + 1); // passive joints and 1 gap joint
-
- /// \brief motor revolutions = encoder value * gap_mechanical_reduction_ * rad2mr_
- /// motor revolutions = motor angle(rad) / (2*pi)
- /// = theta / (2*pi)
- double MR = as[0]->state_.position_ / gap_mechanical_reduction_ * rad2mr_ ;
-
- /// \brief motor revolustions per second = motor angle rate (rad per second) / (2*pi)
- double MR_dot = as[0]->state_.velocity_ / gap_mechanical_reduction_ * rad2mr_ ;
-
- /// \brief gripper motor torque: originally in newton-meters, but we convert it to Nm*(MR/rad)
- /// motor torque = actuator_state->last_meausured_effort
- /// motor torque = I * theta_ddot
- /// MT = I * MR_ddot (my definition)
- /// = I * theta_ddot / (2*pi)
- /// = motot torque / (2*pi)
- double MT = as[0]->state_.last_measured_effort_ / gap_mechanical_reduction_ * rad2mr_ ;
-
- /// internal theta state, gripper closed it is theta0_. same as finger joint angles + theta0_.
- double theta, dtheta_dMR, dt_dtheta, dt_dMR;
- /// information on the fictitious joint: gap_joint
- double gap_size,gap_velocity,gap_effort;
-
- // compute gap position, velocity, applied_effort from actuator states
- computeGapStates(MR,MR_dot,MT,theta,dtheta_dMR, dt_dtheta, dt_dMR,gap_size,gap_velocity,gap_effort);
-
- // assign joint states
- for (unsigned int i = 0; i < js.size(); ++i)
- {
- if (js[i]->joint_->name_ == gap_joint_)
- {
- // assign gap joint
- js[i]->position_ = gap_size*2.0; // function engineering's transmission give half the total gripper size
- js[i]->velocity_ = gap_velocity;
- js[i]->applied_effort_ = gap_effort;
- }
- else
- {
- // find the passive joint name
- std::vector<std::string>::iterator it = std::find(passive_joints_.begin(),passive_joints_.end(),js[i]->joint_->name_);
- if (it != passive_joints_.end())
- {
- // assign passive joints
- js[i]->position_ = theta - theta0_;
- js[i]->velocity_ = dtheta_dMR * MR_dot ;
- js[i]->applied_effort_ = MT / dtheta_dMR ;
- }
- }
- }
-}
-
void PR2GripperTransmission::getRateFromMaxRateJoint(
std::vector<JointState*>& js, std::vector<Actuator*>& as,
int &max_rate_joint_index,double &rate)
@@ -261,15 +220,15 @@
std::vector<std::string>::iterator it = std::find(passive_joints_.begin(),passive_joints_.end(),js[i]->joint_->name_);
if (it != passive_joints_.end())
{
- if (abs(js[i]->velocity_) > max_rate)
+ if (fabs(js[i]->velocity_) > max_rate)
{
- max_rate = abs(js[i]->velocity_) ;
+ max_rate = fabs(js[i]->velocity_) ;
max_rate_joint_index = i;
}
}
}
- assert(max_rate_joint_index < js.size());
+ assert(max_rate_joint_index < (int)js.size());
rate = js[max_rate_joint_index]->velocity_;
}
@@ -282,7 +241,7 @@
// obtain the physical location of passive joints in sim, and average them
angle = 0.0;
- double min_rate = 1.0e16; // a ridiculously large value
+ double min_rate = 1000000000000.0; // a ridiculously large value
torque = 0.0;
min_rate_joint_index = js.size();
@@ -295,21 +254,87 @@
//if (it == passive_joints_.begin())
if (it != passive_joints_.end())
{
- if (abs(js[i]->velocity_) < min_rate)
+ if (fabs(js[i]->velocity_) < min_rate)
{
- min_rate = abs(js[i]->velocity_) ;
+ min_rate = fabs(js[i]->velocity_) ;
min_rate_joint_index = i;
}
}
}
- assert(min_rate_joint_index < js.size()); // some joint rate better be smaller than 1.0e16
+ assert(min_rate_joint_index < (int)js.size()); // some joint rate better be smaller than 1.0e16
angle = angles::shortest_angular_distance(theta0_,js[min_rate_joint_index]->position_) + theta0_;
rate = js[min_rate_joint_index]->velocity_;
torque = js[min_rate_joint_index]->applied_effort_;
}
+///////////////////////////////////////////////////////////
+/// assign joint position, velocity, effort from actuator state
+/// all passive joints are assigned by single actuator state through mimic?
+void PR2GripperTransmission::propagatePosition(
+ std::vector<Actuator*>& as, std::vector<JointState*>& js)
+{
+
+ ROS_ASSERT(as.size() == 1);
+ ROS_ASSERT(js.size() == passive_joints_.size() + 1); // passive joints and 1 gap joint
+
+ /// \brief motor revolutions = encoder value * gap_mechanical_reduction_ * rad2mr_
+ /// motor revolutions = motor angle(rad) / (2*pi)
+ /// = theta / (2*pi)
+ double MR = as[0]->state_.position_ / gap_mechanical_reduction_ * rad2mr_ ;
+
+ /// \brief motor revolustions per second = motor angle rate (rad per second) / (2*pi)
+ double MR_dot = as[0]->state_.velocity_ / gap_mechanical_reduction_ * rad2mr_ ;
+
+ ///
+ /// old MT definition - obsolete
+ ///
+ /// but we convert it to Nm*(MR/rad)
+ /// motor torque = actuator_state->last_meausured_effort
+ /// motor torque = I * theta_ddot
+ /// MT = I * MR_ddot (my definition)
+ /// = I * theta_ddot / (2*pi)
+ /// = motot torque / (2*pi)
+ //double MT = as[0]->state_.last_measured_effort_ / gap_mechanical_reduction_ * rad2mr_ ;
+
+
+ /// \brief gripper motor torque: received from hardware side in newton-meters
+ double MT = as[0]->state_.last_measured_effort_ / gap_mechanical_reduction_;
+
+ /// internal theta state, gripper closed it is theta0_. same as finger joint angles + theta0_.
+ double theta, dtheta_dMR, dt_dtheta, dt_dMR;
+ /// information on the fictitious joint: gap_joint
+ double gap_size,gap_velocity,gap_effort;
+
+ // compute gap position, velocity, applied_effort from actuator states
+ computeGapStates(MR,MR_dot,MT,theta,dtheta_dMR, dt_dtheta, dt_dMR,gap_size,gap_velocity,gap_effort);
+
+ // assign joint states
+ for (unsigned int i = 0; i < js.size(); ++i)
+ {
+ if (js[i]->joint_->name_ == gap_joint_)
+ {
+ // assign gap joint
+ js[i]->position_ = gap_size*2.0; // function engineering's transmission give half the total gripper size
+ js[i]->velocity_ = gap_velocity*2.0;
+ js[i]->applied_effort_ = gap_effort/2.0;
+ }
+ else
+ {
+ // find the passive joint name, set passive joint states
+ std::vector<std::string>::iterator it = std::find(passive_joints_.begin(),passive_joints_.end(),js[i]->joint_->name_);
+ if (it != passive_joints_.end())
+ {
+ // assign passive joint states
+ js[i]->position_ = theta - theta0_;
+ js[i]->velocity_ = dtheta_dMR * MR_dot;
+ js[i]->applied_effort_ = MT / dtheta_dMR / rad2mr_;
+ }
+ }
+ }
+}
+
// this is needed for simulation, so we can recover encoder value given joint angles
void PR2GripperTransmission::propagatePositionBackwards(
std::vector<JointState*>& js, std::vector<Actuator*>& as)
@@ -343,11 +368,10 @@
/// = theta_dot * dMR_dtheta * gap_mechanical_reduction_ / rad2mr
as[0]->state_.velocity_ = joint_rate * dMR_dtheta * gap_mechanical_reduction_ / rad2mr_ ;
- /// state effort = MT * gap_mechanical_reduction_ / rad2mr
- /// = I * MR_ddot * gap_mechanical_reduction_ / rad2mr
- /// = MT * gap_mechanical_reduction_ / rad2mr
- /// = gap_effort * dt_dMR * gap_mechanical_reduction_ / rad2mr
- as[0]->state_.last_measured_effort_ = gap_effort / dMR_dt * gap_mechanical_reduction_ / rad2mr_ ;
+ /// motor torque = inverse of getting gap effort from motor torque
+ /// = gap_effort * dt_dMR / (2*pi) * gap_mechanical_reduction_
+ /// = gap_effort / dMR_dt * rad2mr_ * gap_mechanical_reduction_
+ as[0]->state_.last_measured_effort_ = gap_effort / dMR_dt * rad2mr_ * gap_mechanical_reduction_;
return;
}
}
@@ -384,10 +408,10 @@
{
if (js[i]->joint_->name_ == gap_joint_)
{
- double gap_effort = js[i]->commanded_effort_; /// Newtons
- /// actuator commanded effort = MT * gap_mechanical_reduction_ / rad2mr_
- /// = gap_dffort / dMR_dt * gap_mechanical_reduction_ / rad2mr_
- as[0]->command_.effort_ = gap_effort / dMR_dt * gap_mechanical_reduction_ / rad2mr_ ;
+ double gap_effort = js[i]->commanded_effort_; /// Newtons
+ /// actuator commanded effort = gap_dffort / dMR_dt / (2*pi) * gap_mechanical_reduction_
+ as[0]->command_.effort_ = 2.0 * gap_effort / dMR_dt * rad2mr_ * gap_mechanical_reduction_; // divide by 2 because torque is transmitted to 2 fingers
+ //ROS_WARN("debug: %f %f %f",gap_effort,dMR_dt,rad2mr_);
break;
}
}
@@ -407,7 +431,7 @@
/// \brief taken from propagatePosition()
double MR = as[0]->state_.position_ / gap_mechanical_reduction_ * rad2mr_ ;
double MR_dot = as[0]->state_.velocity_ / gap_mechanical_reduction_ * rad2mr_ ;
- double MT = as[0]->command_.effort_ / gap_mechanical_reduction_ * rad2mr_ ;
+ double MT = as[0]->command_.effort_ / gap_mechanical_reduction_;
/// internal theta state, gripper closed it is theta0_. same as finger joint angles + theta0_.
double theta, dtheta_dMR, dt_dtheta, dt_dMR;
@@ -424,6 +448,7 @@
{
// propagate fictitious joint effort backwards
js[i]->commanded_effort_ = gap_effort;
+ //ROS_WARN("debug2: %f %f %f",gap_effort,dt_dMR,rad2mr_);
}
else
{
@@ -446,13 +471,13 @@
// FIXME: hackery, due to transmission values, MT is too large for the damping available
// with the given time step size in sim, so until implicit damping is implemented,
- // we'll scale MT with inverse of velocity
- //double scale = exp(-abs(js[i]->velocity_*1.0));
+ // we'll scale MT with inverse of velocity to some power
int max_joint_rate_index;
- double scale,max_joint_rate;
+ double scale=1.0,rate_threshold=0.5;
+ double max_joint_rate;
getRateFromMaxRateJoint(js, as, max_joint_rate_index, max_joint_rate);
- scale = (abs(max_joint_rate)>0.0) ? 1./abs(max_joint_rate*0000001.0) : 1.0;
-
+ if (fabs(max_joint_rate)>rate_threshold) scale /= pow(fabs(max_joint_rate/rate_threshold),2.0);
+ //std::cout << "rate " << max_joint_rate << " absrate " << fabs(max_joint_rate) << " scale " << scale << std::endl;
// sum joint torques from actuator motor and mimic constraint and convert to joint torques
js[i]->commanded_effort_ = scale*MT / dtheta_dMR;
}
Modified: pkg/trunk/stacks/pr2/pr2_defs/defs/gripper_defs.xml
===================================================================
--- pkg/trunk/stacks/pr2/pr2_defs/defs/gripper_defs.xml 2009-07-27 23:30:10 UTC (rev 19769)
+++ pkg/trunk/stacks/pr2/pr2_defs/defs/gripper_defs.xml 2009-07-27 23:42:46 UTC (rev 19770)
@@ -415,13 +415,13 @@
- <macro name="pr2_gripper" params="side parent">
+ <macro name="pr2_gripper" params="side parent *origin gear_ratio screw_reduction">
<joint name="${side}_gripper_palm_joint" type="fixed" />
<link name="${side}_gripper_palm_link">
<parent name="${parent}" />
<joint name="${side}_gripper_palm_joint" />
- <origin xyz="0 0 0" rpy="0 0 0" />
+ <insert_block name="origin" />
<inertial>
<mass value="0.58007" />
<com xyz="0.06623 0.00053 -0.00119" />
@@ -609,19 +609,11 @@
<!-- gazebo_mimic_pid is for sim only. -->
<transmission type="PR2GripperTransmission" name="${side}_gripper_trans" >
<actuator name="${side}_gripper_motor" />
- <gap_joint name="${side}_gripper_joint" mechanical_reduction="1.0" />
- <passive_joint name="${side}_gripper_l_finger_joint" >
- <gazebo_mimic_pid p="0.0000" i="0.000" d="0.0" iClamp="0.0" />
- </passive_joint>
- <passive_joint name="${side}_gripper_r_finger_joint" >
- <gazebo_mimic_pid p="0.0000" i="0.000" d="0.0" iClamp="0.0" />
- </passive_joint>
- <passive_joint name="${side}_gripper_r_finger_tip_joint" >
- <gazebo_mimic_pid p="0.000000" i="0.0" d="0.0" iClamp="0.0" />
- </passive_joint>
- <passive_joint name="${side}_gripper_l_finger_tip_joint" >
- <gazebo_mimic_pid p="0.000000" i="0.0" d="0.0" iClamp="0.0" />
- </passive_joint>
+ <gap_joint name="${side}_gripper_joint" screw_reduction="${screw_reduction}" gear_ratio="${gear_ratio}" mechanical_reduction="1.0" />
+ <passive_joint name="${side}_gripper_l_finger_joint" />
+ <passive_joint name="${side}_gripper_r_finger_joint" />
+ <passive_joint name="${side}_gripper_r_finger_tip_joint" />
+ <passive_joint name="${side}_gripper_l_finger_tip_joint" />
</transmission>
<!--
@@ -643,232 +635,13 @@
</macro>
-<!-- Alpha 2.0 gripper. -->
-<!-- Only change is that gripper is moved out in the x-dir 3mm from wrist. -->
- <macro name="pr2_gripper_alpha2" params="side parent">
-
- <joint name="${side}_gripper_palm_joint" type="fixed" />
- <link name="${side}_gripper_palm_link">
- <parent name="${parent}" />
- <joint name="${side}_gripper_palm_joint" />
- <!-- New origin for palm joint for Alpha 2.0 gripper -->
- <origin xyz="0.003 0 0" rpy="0 0 0" />
- <inertial>
- <mass value="0.58007" />
- <com xyz="0.06623 0.00053 -0.00119" />
- <inertia ixx="0.00035223921" ixy="-0.00001580476" ixz="-0.00000091750"
- iyy="0.00067741312" iyz="-0.00000059554"
- izz="0.00086563316" />
- </inertial>
-
- <visual>
- <origin xyz="0 0 0" rpy="0 0 0" />
- <map name="gazebo_material" flag="gazebo">
- <elem key="material" value="PR2/Red" />
- </map>
- <geometry name="${side}_gripper_palm_visual">
- <mesh filename="gripper_palm.stlb" />
- </geometry>
- <map name="${side}_gripper_palm_visual_mesh" flag="ogre">
- <elem key="mesh" value="gripper_palm.mesh" />
- </map>
- </visual>
- <collision>
- <origin xyz="0 0 0" rpy="0 0 0" />
- <geometry name="${side}_gripper_palm_collision">
- <mesh filename="convex/gripper_palm_convex.stlb" />
- </geometry>
- <map name="${side}_gripper_palm_collision_mesh" flag="ogre">
- <elem key="mesh" value="convex/gripper_palm_convex.mesh" />
- </map>
- <verbose value="Yes" />
- <map flag="collision" name="mesh">
- <elem key="type" value="visual"/>
- </map>
- </collision>
- <map name="${side}_gripper_palm_sensor" flag="gazebo">
- <verbatim key="${side}_gripper_palm_bumper_sensor">
- <sensor:contact name="${side}_gripper_palm_contact_sensor">
- <geom>${side}_gripper_palm_collision</geom>
- <updateRate>100.0</updateRate>
- <controller:ros_bumper name="${side}_gripper_palm_ros_bumper_controller" plugin="libros_bumper.so">
- <alwaysOn>true</alwaysOn>
- <updateRate>100.0</updateRate>
- <bumperTopicName>${side}_gripper_palm_bumper</bumperTopicName>
- <interface:bumper name="${side}_gripper_palm_ros_bumper_iface" />
- </controller:ros_bumper>
- </sensor:contact>
- </verbatim>
- </map>
- <map name="${side}_gripper_palm_gravity" flag="gazebo">
- <elem key="turnGravityOff">true</elem>
- </map>
- </link>
-
-
-
- <!-- floating link for simulating gripper constraints -->
- <joint name="${side}_gripper_float_joint" type="prismatic">
- <limit min="-0.05" max="0.001" effort="0"
- safety_length_min="0.0" safety_length_max="0.0" />
- <axis xyz="1 0 0" />
- <joint_properties damping="10.0" />
- </joint>
- <link name="${side}_gripper_float_link">
- <parent name="${side}_gripper_palm_link" />
- <joint name="${side}_gripper_float_joint" />
- <origin xyz="0.05 0 0" rpy="0 0 0" />
- <inertial>
- <mass value="0.01" />
- <com xyz="0 0 0" />
- <inertia ixx="0.0001" ixy="0" ixz="0"
- iyy="0.0001" iyz="0"
- izz="0.0001" />
- </inertial>
- <visual>
- <origin xyz="0 0 0" rpy="0 0 0" />
- <map name="gazebo_material" flag="gazebo">
- <elem key="material" value="PR2/Red" />
- </map>
- <geometry name="${side}_gripper_float_visual">
- <box size="0.1 0.1 0.1" />
- </geometry>
- </visual>
- <collision>
- <origin xyz="0 0 0" rpy="0 0 0" />
- <geometry name="${side}_gripper_float_collision">
- <box size="0.001 0.001 0.001" />
- </geometry>
- <verbose value="Yes" />
- <map flag="collision" name="mesh">
- <elem key="type" value="visual"/>
- </map>
- </collision>
- <map name="${side}_gripper_float_gravity" flag="gazebo">
- <elem key="turnGravityOff">true</elem>
- </map>
- </link>
- <map name="${side}_fake_gripper_slider_joint" flag="gazebo">
- <verbatim key="${side}_fake_gripper_slider_joint">
- <joint:slider name="${side}_gripper_l_finger_tip_slider_joint">
- <body1>${side}_gripper_l_finger_tip_link</body1>
- <body2>${side}_gripper_float_link</body2>
- <anchor>${side}_gripper_l_finger_tip_link</anchor>
- <axis>0 1 0</axis>
- <anchorOffset>0 0 0</anchorOffset>
- </joint:slider>
- <joint:slider name="${side}_gripper_r_finger_tip_slider_joint">
- <body1>${side}_gripper_r_finger_tip_link</body1>
- <body2>${side}_gripper_float_link</body2>
- <anchor>${side}_gripper_r_finger_tip_link</anchor>
- <axis>0 1 0</axis>
- <anchorOffset>0 0 0</anchorOffset>
- </joint:slider>
- </verbatim>
- </map>
-
-
-
-
- <map name="sensor" flag="gazebo">
- <verbatim key="p3d_${side}_gripper_palm">
- <controller:ros_p3d name="p3d_${side}_gripper_palm_controller" plugin="libros_p3d.so">
- <alwaysOn>true</alwaysOn>
- <updateRate>100.0</updateRate>
- <bodyName>${side}_gripper_palm_link</bodyName>
- <topicName>${side}_gripper_palm_pose_ground_truth</topicName>
- <xyzOffsets>0 0 0</xyzOffsets>
- <rpyOffsets>0 0 0</rpyOffsets>
- <gaussianNoise>0.0</gaussianNoise>
- <frameName>map</frameName>
- <interface:position name="p3d_${side}_gripper_palm_position_iface" />
- </controller:ros_p3d>
- </verbatim>
- </map>
-
- <joint name="${side}_gripper_tool_joint" type="fixed" />
- <link name="${side}_gripper_tool_frame">
- <parent name="${side}_gripper_palm_link" />
- <joint name="${side}_gripper_tool_joint" />
- <origin xyz="0.18 0 0" rpy="0 0 0" />
-
- <!-- TODO: all that follows is made-up and useless, but the old parser crashes without it -->
- <inertial>
- <mass value="0" />
- <com xyz="0 0 0" />
- <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
- </inertial>
- <visual>
- <origin xyz="0 0 0" rpy="0 0 0" />
- <map name="gazebo_material" flag="gazebo">
- <elem key="material" value="PR2/Grey" />
- </map>
- <geometry name="${side}_gripper_tool_visual">
- <box size="0.001 0.001 0.001" />
- </geometry>
- </visual>
- <collision>
- <origin xyz="-0.1 0 0" rpy="0 0 0" /> <!-- move it out of the way -->
- <geometry name="${side}_gripper_tool_collision">
- <box size="0.001 0.001 0.001" />
- </geometry>
- </collision>
- <map name="${side}_gripper_tool_gravity" flag="gazebo">
- <elem key="turnGravityOff">true</elem>
- </map>
- </link>
-
- <pr2_finger prefix="${side}_gripper" parent="${side}_gripper_palm_link" />
-
- <!-- [lr]_gripper_joint position is the gap_size/2 -->
- <!-- [lr]_gripper_joint velocity is the gap linear velocity -->
- <!-- [lr]_gripper_joint effort is the gap linear force -->
- <!-- Please refer to function engineering spreadsheet 090224_link_data.xls for -->
- <!-- the transmission function. -->
- <!-- Please refer to mechanism_model/src/pr2_gripper_transmission.cpp for implementation.-->
- <!-- gazebo_mimic_pid is for sim only. -->
- <transmission type="PR2GripperTransmission" name="${side}_gripper_trans" >
- <actuator name="${side}_gripper_motor" />
- <gap_joint name="${side}_gripper_joint" mechanical_reduction="1.0" />
- <passive_joint name="${side}_gripper_l_finger_joint" >
- <gazebo_mimic_pid p="0.0000" i="0.000" d="0.0" iClamp="0.0" />
- </passive_joint>
- <passive_joint name="${side}_gripper_r_finger_joint" >
- <gazebo_mimic_pid p="0.0000" i="0.000" d="0.0" iClamp="0.0" />
- </passive_joint>
- <passive_joint name="${side}_gripper_r_finger_tip_joint" >
- <gazebo_mimic_pid p="0.00000" i="0.0" d="0.0" iClamp="0.0" />
- </passive_joint>
- <passive_joint name="${side}_gripper_l_finger_tip_joint" >
- <gazebo_mimic_pid p="0.00000" i="0.0" d="0.0" iClamp="0.0" />
- </passive_joint>
- </transmission>
-
- <!--
- min="${gripper_min_angle}" max="${gripper_max_angle}"
- -->
- <!-- fictitous joint that represents the gripper gap -->
- <!-- effort is the linear force at the gripper gap
- velocity limit is the linear velocity limit at the gripper gap
- try and introduce a very stiff spring
- -->
- <joint name="${side}_gripper_joint" type="prismatic">
- <axis xyz="0 0 1" />
- <limit effort="100.0" velocity="0.02"
- min="0.0" max="0.09"
- k_velocity="500.0" k_position="20.0"
- safety_length_min="0.0" safety_length_max="0.001" />
- </joint>
-
- </macro>
-
<!-- Calibration -->
<macro name="gripper_calibrator" params="side">
<controller name="cal_${side}_gripper" type="GripperCalibrationControllerNode">
<calibrate joint="${side}_gripper_joint"
actuator="${side}_gripper_motor"
velocity="-0.01" />
- <pid p="1500" i="25.0" d="0" iClamp="100.0" />
+ <pid p="15000" i="25.0" d="0" iClamp="100.0" />
</controller>
</macro>
Modified: pkg/trunk/stacks/pr2/pr2_defs/robots/l_arm.xacro.xml
===================================================================
--- pkg/trunk/stacks/pr2/pr2_defs/robots/l_arm.xacro.xml 2009-07-27 23:30:10 UTC (rev 19769)
+++ pkg/trunk/stacks/pr2/pr2_defs/robots/l_arm.xacro.xml 2009-07-27 23:42:46 UTC (rev 19770)
@@ -27,7 +27,9 @@
<pr2_arm side="l" reflect="1" parent="torso_lift_link">
<origin xyz="0.0 0.0 1.0" rpy="0 0 0" />
</pr2_arm>
- <pr2_gripper side="l" parent="l_wrist_roll_link" />
+ <pr2_gripper screw_reduction="${2.0/1000.0}" gear_ratio="${29.16}" side="l" parent="l_wrist_roll_link" >
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ </pr2_gripper>
<!-- Solid Base -->
<joint name="base_joint" type="planar" />
Modified: pkg/trunk/stacks/pr2/pr2_defs/robots/l_gripper.xacro.xml
===================================================================
--- pkg/trunk/stacks/pr2/pr2_defs/robots/l_gripper.xacro.xml 2009-07-27 23:30:10 UTC (rev 19769)
+++ pkg/trunk/stacks/pr2/pr2_defs/robots/l_gripper.xacro.xml 2009-07-27 23:42:46 UTC (rev 19770)
@@ -21,7 +21,10 @@
<include filename="$(find pr2_defs)/defs/gripper_defs.xml" />
<include filename="$(find pr2_defs)/defs/gazebo_defs.xml" />
- <pr2_gripper side="l" parent="base_link" />
+ <!-- for new gripper gear_ratio="29.16*22.0/16.0*0.9" screw_reduction="${4.0/1000.0}" -->
+ <pr2_gripper screw_reduction="${2.0/1000.0}" gear_ratio="${29.16}" side="l" parent="base_link" >
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ </pr2_gripper>
<!-- Solid Base -->
<joint name="base_joint" type="planar">
Modified: pkg/trunk/stacks/pr2/pr2_defs/robots/pr2.xacro.xml
===================================================================
--- pkg/trunk/stacks/pr2/pr2_defs/robots/pr2.xacro.xml 2009-07-27 23:30:10 UTC (rev 19769)
+++ pkg/trunk/stacks/pr2/pr2_defs/robots/pr2.xacro.xml 2009-07-27 23:42:46 UTC (rev 19770)
@@ -46,12 +46,16 @@
<pr2_arm side="r" reflect="-1" parent="torso_lift_link">
<origin xyz="0 -0.188 0.0" rpy="0 0 0" />
</pr2_arm>
- <pr2_gripper side="r" parent="r_wrist_roll_link" />
+ <pr2_gripper screw_reduction="${2.0/1000.0}" gear_ratio="${29.16}" side="r" parent="r_wrist_roll_link" >
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ </pr2_gripper>
<pr2_arm side="l" reflect="1" parent="torso_lift_link">
<origin xyz="0.0 0.188 0.0" rpy="0 0 0" />
</pr2_arm>
- <pr2_gripper side="l" parent="l_wrist_roll_link" />
+ <pr2_gripper screw_reduction="${2.0/1000.0}" gear_ratio="${29.16}" side="l" parent="l_wrist_roll_link" >
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ </pr2_gripper>
<!--pr2_ptz side="r" reflect="-1" parent="torso_lift_link">
<origin xyz="0.0 -0.1975 0.2265" rpy="0 0 0" />
Modified: pkg/trunk/stacks/pr2/pr2_defs/robots/pre.xacro.xml
===================================================================
--- pkg/trunk/stacks/pr2/pr2_defs/robots/pre.xacro.xml 2009-07-27 23:30:10 UTC (rev 19769)
+++ pkg/trunk/stacks/pr2/pr2_defs/robots/pre.xacro.xml 2009-07-27 23:42:46 UTC (rev 19770)
@@ -55,12 +55,16 @@
<pr2_arm side="r" reflect="-1" parent="torso_lift_link">
<origin xyz="0 -0.188 0.0" rpy="0 0 0" />
</pr2_arm>
- <pr2_gripper side="r" parent="r_wrist_roll_link" />
+ <pr2_gripper screw_reduction="${2.0/1000.0}" gear_ratio="${29.16}" side="r" parent="r_wrist_roll_link" >
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ </pr2_gripper>
<pr2_arm side="l" reflect="1" parent="torso_lift_link">
<origin xyz="0.0 0.188 0.0" rpy="0 0 0" />
</pr2_arm>
- <pr2_gripper side="l" parent="l_wrist_roll_link" />
+ <pr2_gripper screw_reduction="${2.0/1000.0}" gear_ratio="${29.16}" side="l" parent="l_wrist_roll_link" >
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ </pr2_gripper>
<pr2_ptz side="r" reflect="-1" parent="torso_lift_link">
<origin xyz="0.0 -0.1975 0.2265" rpy="0 0 0" />
Modified: pkg/trunk/stacks/pr2/pr2_defs/robots/prf.xacro.xml
===================================================================
--- pkg/trunk/stacks/pr2/pr2_defs/robots/prf.xacro.xml 2009-07-27 23:30:10 UTC (rev 19769)
+++ pkg/trunk/stacks/pr2/pr2_defs/robots/prf.xacro.xml 2009-07-27 23:42:46 UTC (rev 19770)
@@ -47,13 +47,17 @@
<pr2_arm side="r" reflect="-1" parent="torso_lift_link">
<origin xyz="0 -0.188 0.0" rpy="0 0 0" />
</pr2_arm>
- <pr2_gripper side="r" parent="r_wrist_roll_link" />
+ <pr2_gripper screw_reduction="${2.0/1000.0}" gear_ratio="${29.16}" side="r" parent="r_wrist_roll_link" >
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ </pr2_gripper>
<!--
<pr2_arm side="l" reflect="1" parent="torso_lift_link">
<origin xyz="0.0 0.188 0.0" rpy="0 0 0" />
</pr2_arm>
- <pr2_gripper side="l" parent="l_wrist_roll_link" />
+ <pr2_gripper screw_reduction="${2.0/1000.0}" gear_ratio="${29.16}" side="l" parent="l_wrist_roll_link" />
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ </pr2_gripper>
-->
<pr2_ptz side="r" reflect="-1" parent="torso_lift_link">
Modified: pkg/trunk/stacks/pr2/pr2_defs/robots/prg.xacro.xml
===================================================================
--- pkg/trunk/stacks/pr2/pr2_defs/robots/prg.xacro.xml 2009-07-27 23:30:10 UTC (rev 19769)
+++ pkg/trunk/stacks/pr2/pr2_defs/robots/prg.xacro.xml 2009-07-27 23:42:46 UTC (rev 19770)
@@ -47,13 +47,17 @@
<pr2_arm side="r" reflect="-1" parent="torso_lift_link">
<origin xyz="0 -0.188 0.0" rpy="0 0 0" />
</pr2_arm>
- <pr2_gripper side="r" parent="r_wrist_roll_link" />
+ <pr2_gripper screw_reduction="${2.0/1000.0}" gear_ratio="${29.16}" side="r" parent="r_wrist_roll_link" >
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ </pr2_gripper>
<!--
<pr2_arm side="l" reflect="1" parent="torso_lift_link">
<origin xyz="0.0 0.188 0.0" rpy="0 0 0" />
</pr2_arm>
- <pr2_gripper side="l" parent="l_wrist_roll_link" />
+ <pr2_gripper screw_reduction="${2.0/1000.0}" gear_ratio="${29.16}" side="l" parent="l_wrist_roll_link" >
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ </pr2_gripper>
-->
<pr2_ptz side="r" reflect="-1" parent="torso_lift_link">
Modified: pkg/trunk/stacks/pr2/pr2_defs/robots/r_arm.xacro.xml
===================================================================
--- pkg/trunk/stacks/pr2/pr2_defs/robots/r_arm.xacro.xml 2009-07-27 23:30:10 UTC (rev 19769)
+++ pkg/trunk/stacks/pr2/pr2_defs/robots/r_arm.xacro.xml 2009-07-27 23:42:46 UTC (rev 19770)
@@ -27,7 +27,9 @@
<pr2_arm side="r" reflect="-1" parent="torso_lift_link">
<origin xyz="0.0 0.0 1.0" rpy="0 0 0" />
</pr2_arm>
- <pr2_gripper side="r" parent="r_wrist_roll_link" />
+ <pr2_gripper screw_reduction="${2.0/1000.0}" gear_ratio="${29.16}" side="r" parent="r_wrist_roll_link" >
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ </pr2_gripper>
<!-- Solid Base -->
<joint name="base_joint" type="planar" />
Modified: pkg/trunk/stacks/pr2/pr2_defs/robots/r_forearm_gripper.xacro.xml
===================================================================
--- pkg/trunk/stacks/pr2/pr2_defs/robots/r_forearm_gripper.xacro.xml 2009-07-27 23:30:10 UTC (rev 19769)
+++ pkg/trunk/stacks/pr2/pr2_defs/robots/r_forearm_gripper.xacro.xml 2009-07-27 23:42:46 UTC (rev 19770)
@@ -23,7 +23,9 @@
<include filename="$(find pr2_defs)/defs/gazebo_defs.xml" />
<pr2_forearm side="r" reflect="1" parent="base_link"/>
- <pr2_gripper side="r" parent="r_wrist_roll_link" />
+ <pr2_gripper screw_reduction="${2.0/1000.0}" gear_ratio="${29.16}" side="r" parent="r_wrist_roll_link" >
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ </pr2_gripper>
<!-- Solid Base -->
<joint name="base_joint" type="planar">
Modified: pkg/trunk/stacks/pr2/pr2_defs/robots/r_gripper.xacro.xml
===================================================================
--- pkg/trunk/stacks/pr2/pr2_defs/robots/r_gripper.xacro.xml 2009-07-27 23:30:10 UTC (rev 19769)
+++ pkg/trunk/stacks/pr2/pr2_defs/robots/r_gripper.xacro.xml 2009-07-27 23:42:46 UTC (rev 19770)
@@ -21,7 +21,9 @@
<include filename="$(find pr2_defs)/defs/gripper_defs.xml" />
<include filename="$(find pr2_defs)/defs/gazebo_defs.xml" />
- <pr2_gripper side="r" parent="base_link" />
+ <pr2_gripper screw_reduction="${2.0/1000.0}" gear_ratio="${29.16}" side="r" parent="base_link" >
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ </pr2_gripper>
<!-- Solid Base -->
<joint name="base_joint" type="planar">
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
