Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[8603] pkg/trunk
|
From: <rdi...@us...> - 2008-12-27 02:14:23
|
Revision: 8603
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=8603&view=rev
Author: rdiankov
Date: 2008-12-27 01:45:36 +0000 (Sat, 27 Dec 2008)
Log Message:
-----------
correct pr2 gripper transmissions now working
Modified Paths:
--------------
pkg/trunk/estimation/robot_pose_ekf/src/odom_estimation_node.h
pkg/trunk/hardware_test/life_test/arm_life_test/controllers_twist.xml
pkg/trunk/hardware_test/life_test/arm_life_test/teleop_arm.launch
pkg/trunk/mechanism/mechanism_model/src/gripper_transmission.cpp
pkg/trunk/robot_descriptions/pr2_alpha/prf.launch
pkg/trunk/robot_descriptions/pr2_alpha/teleop_joystick_new.launch
pkg/trunk/robot_descriptions/wg_robot_description/pr2_robot_defs/gripper_defs.xml
Modified: pkg/trunk/estimation/robot_pose_ekf/src/odom_estimation_node.h
===================================================================
--- pkg/trunk/estimation/robot_pose_ekf/src/odom_estimation_node.h 2008-12-27 01:44:57 UTC (rev 8602)
+++ pkg/trunk/estimation/robot_pose_ekf/src/odom_estimation_node.h 2008-12-27 01:45:36 UTC (rev 8603)
@@ -50,6 +50,8 @@
#include "robot_msgs/VOPose.h"
#include "robot_msgs/PoseWithCovariance.h"
+#include <rosthread/mutex.h>
+
// log files
#include <fstream>
Modified: pkg/trunk/hardware_test/life_test/arm_life_test/controllers_twist.xml
===================================================================
--- pkg/trunk/hardware_test/life_test/arm_life_test/controllers_twist.xml 2008-12-27 01:44:57 UTC (rev 8602)
+++ pkg/trunk/hardware_test/life_test/arm_life_test/controllers_twist.xml 2008-12-27 01:45:36 UTC (rev 8603)
@@ -2,6 +2,6 @@
<controller type="EndeffectorTwistControllerNode" name="arm_twist" topic="arm_twist">
<pid_trans p="20.0" d="0.0" i="0.5" iClamp="1.0" />
<pid_rot p="0.5" d="0.0" i="0.1" iClamp="0.2" />
- <chain root="torso_link" tip="r_wrist_roll_link" offset="0.3 0 0" />
+ <chain root="torso_lift_link" tip="r_wrist_roll_link" offset="0.3 0 0" />
</controller>
</controllers>
Modified: pkg/trunk/hardware_test/life_test/arm_life_test/teleop_arm.launch
===================================================================
--- pkg/trunk/hardware_test/life_test/arm_life_test/teleop_arm.launch 2008-12-27 01:44:57 UTC (rev 8602)
+++ pkg/trunk/hardware_test/life_test/arm_life_test/teleop_arm.launch 2008-12-27 01:45:36 UTC (rev 8603)
@@ -3,7 +3,7 @@
<!-- Arm twist controller -->
<param name="arm_twist/joystick_max_trans" value="3.0" />
<param name="arm_twist/joystick_max_rot" value="4.0" />
- <node pkg="mechanism_control" type="spawner.py" args="controllers_twist.xml" />
+ <node pkg="mechanism_control" type="spawner.py" args="$(find arm_life_test)/controllers_twist.xml" />
<!-- Gripper controller -->
Modified: pkg/trunk/mechanism/mechanism_model/src/gripper_transmission.cpp
===================================================================
--- pkg/trunk/mechanism/mechanism_model/src/gripper_transmission.cpp 2008-12-27 01:44:57 UTC (rev 8602)
+++ pkg/trunk/mechanism/mechanism_model/src/gripper_transmission.cpp 2008-12-27 01:45:36 UTC (rev 8603)
@@ -107,25 +107,22 @@
ROS_ASSERT(as.size() == 1);
ROS_ASSERT(js.size() == preductions_.size());
- // cos(pos*reduction) = A*motor+B
- // -reduction * sin(pos*reduction) * dpos/dt = A * dmotor/dt
- double cang = as[0]->state_.position_*A_+B_;
+ // sin(pos*reduction) = A*motor+B
+ // reduction * cos(pos*reduction) * dpos/dt = A * dmotor/dt
+ double sang = as[0]->state_.position_*A_+B_;
double ang;
- if( cang < -1 )
- ang = M_PI;
- else if( cang > 1 )
- ang = 0;
+ if( sang <= -1 )
+ ang = -M_PI/2;
+ else if( sang >= 1 )
+ ang = M_PI/2;
else
- ang = acos(cang);
+ ang = asin(sang);
for (unsigned int i = 0; i < js.size(); ++i)
{
js[i]->position_ = ang / preductions_[i];
- js[i]->velocity_ = - A_*as[0]->state_.velocity_ / (sin(ang)*preductions_[i]);
+ js[i]->velocity_ = A_*as[0]->state_.velocity_ / (cos(ang)*preductions_[i]);
js[i]->applied_effort_ = as[0]->state_.last_measured_effort_ * ereductions_[i];
- ROS_INFO("gripper pos %d: %f from %f (%f,%f)\n", i, js[i]->position_, as[0]->state_.position_, (float)A_, (float)B_);
- js[i]->position_ = as[0]->state_.position_;
- js[i]->velocity_ = as[0]->state_.velocity_;
}
}
@@ -140,14 +137,13 @@
double mean_effort = 0.0;
for (unsigned int i = 0; i < js.size(); ++i)
{
- mean_position += (cos(js[i]->position_ * preductions_[i])-B_)/A_;
- mean_velocity += - js[i]->velocity_ * preductions_[i] * sin(js[i]->position_*preductions_[i]) / A_;
+ mean_position += (sin(js[i]->position_ * preductions_[i])-B_)/A_;
+ mean_velocity += js[i]->velocity_ * preductions_[i] * cos(js[i]->position_*preductions_[i]) / A_;
mean_effort += js[i]->applied_effort_ / (ereductions_[i]);
}
- ROS_INFO("gripper motor pos: %f\n", (float)mean_position / js.size());
- as[0]->state_.position_ = js[0]->position_;//mean_position / js.size();
- as[0]->state_.velocity_ = js[0]->velocity_;//mean_velocity / js.size();
+ as[0]->state_.position_ = mean_position / js.size();
+ as[0]->state_.velocity_ = mean_velocity / js.size();
as[0]->state_.last_measured_effort_ = mean_effort / js.size();
}
@@ -164,7 +160,6 @@
strongest = js[i]->commanded_effort_ / ereductions_[i];
}
- //ROS_INFO("gripper motor eff: %f\n", (float)strongest);
as[0]->command_.effort_ = strongest;
}
@@ -177,7 +172,7 @@
std::vector<double> scaled_positions(js.size());
for (unsigned int i = 0; i < js.size(); ++i)
- scaled_positions[i] = (cos(js[i]->position_ * preductions_[i])-B_)/A_;
+ scaled_positions[i] = (sin(js[i]->position_ * preductions_[i])-B_)/A_;
double mean = std::accumulate(scaled_positions.begin(), scaled_positions.end(), 0.0)
/ scaled_positions.size();
@@ -187,7 +182,6 @@
double err = scaled_positions[i] - mean;
double pid_effort = pids_[i].updatePid(err, 0.001);
- ROS_INFO("gripper commanded eff: %f\n", (float)as[0]->command_.effort_ * ereductions_[i]);
js[i]->commanded_effort_ =
/*pid_effort / ereductions_[i] + */as[0]->command_.effort_ * ereductions_[i];
}
Modified: pkg/trunk/robot_descriptions/pr2_alpha/prf.launch
===================================================================
--- pkg/trunk/robot_descriptions/pr2_alpha/prf.launch 2008-12-27 01:44:57 UTC (rev 8602)
+++ pkg/trunk/robot_descriptions/pr2_alpha/prf.launch 2008-12-27 01:45:36 UTC (rev 8603)
@@ -63,12 +63,12 @@
-->
<!-- DCAM-->
- <node machine="three" name="dcam" pkg="dcam_node" type="dcam_node" respawn="false">
+<!-- <node machine="three" name="dcam" pkg="dcam_node" type="dcam_node" respawn="false">
<param name="video_mode" type="str" value="640x480_videre_none"/>
<param name="do_rectify" type="bool" value="True"/>
<param name="do_stereo" type="bool" value="True"/>
<param name="do_calc_points" type="bool" value="False"/>
- </node>
+ </node> -->
<!-- Runtime Diagnostics Logging -->
Modified: pkg/trunk/robot_descriptions/pr2_alpha/teleop_joystick_new.launch
===================================================================
--- pkg/trunk/robot_descriptions/pr2_alpha/teleop_joystick_new.launch 2008-12-27 01:44:57 UTC (rev 8602)
+++ pkg/trunk/robot_descriptions/pr2_alpha/teleop_joystick_new.launch 2008-12-27 01:45:36 UTC (rev 8603)
@@ -1,6 +1,6 @@
<launch>
<param name="base_controller/odom_publish_rate" value="30.0"/>
-<node pkg="mechanism_control" type="spawner.py" args="controllers_base_new.xml" output="screen"/>
+<node pkg="mechanism_control" type="spawner.py" args="$(find pr2_alpha)/controllers_base_new.xml" output="screen"/>
<group name="wg">
<param name="axis_vx" value="3" type="int"/>
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-27 01:44:57 UTC (rev 8602)
+++ pkg/trunk/robot_descriptions/wg_robot_description/pr2_robot_defs/gripper_defs.xml 2008-12-27 01:45:36 UTC (rev 8603)
@@ -5,7 +5,7 @@
<property name="M_PI" value="3.1415926535897931" />
- <property name="gripper_max_angle" value="0.5236" />
+ <property name="gripper_max_angle" value="0.548" />
<property name="gripper_min_angle" value="0.05" />
<macro name="pr2_finger" params="prefix parent">
@@ -423,20 +423,20 @@
<pr2_finger prefix="${side}_gripper" parent="${side}_gripper_palm" />
- <!-- cos(angle*reduction) = motor_encoder * A + B -->
-<!-- <transmission type="GripperTransmission" name="${side}_gripper_trans" A="0.00042803" B="0.97567">
+ <!-- sin(angle*reduction) = motor_encoder * A + B -->
+ <transmission type="GripperTransmission" name="${side}_gripper_trans" A="0.0002193927" B="0.530981135">
<actuator name="${side}_gripper_motor" />
- <joint name="${side}_gripper_r_finger_joint" preduction="1.0" ereduction="6.0" />
- <joint name="${side}_gripper_r_finger_tip_joint" preduction="-1.0" ereduction="-6.0" />
- <joint name="${side}_gripper_l_finger_joint" preduction="-1.0" ereduction="-6.0"/>
- <joint name="${side}_gripper_l_finger_tip_joint" preduction="1.0" ereduction="6.0" />
+ <joint name="${side}_gripper_r_finger_joint" preduction="-1.0" ereduction="-6.0" />
+ <joint name="${side}_gripper_r_finger_tip_joint" preduction="1.0" ereduction="6.0" />
+ <joint name="${side}_gripper_l_finger_joint" preduction="1.0" ereduction="6.0"/>
+ <joint name="${side}_gripper_l_finger_tip_joint" preduction="-1.0" ereduction="-6.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.
|
