Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[13883] pkg/trunk
|
From: <stu...@us...> - 2009-04-15 20:23:04
|
Revision: 13883
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=13883&view=rev
Author: stuglaser
Date: 2009-04-15 20:22:53 +0000 (Wed, 15 Apr 2009)
Log Message:
-----------
Better velocity saturation for the hybrid controller
Modified Paths:
--------------
pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_hybrid_controller.cpp
pkg/trunk/robot_descriptions/pr2/pr2_default_controllers/gains/arm_pose.yaml
pkg/trunk/robot_descriptions/pr2/pr2_default_controllers/gains/arm_twist.yaml
Modified: pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_hybrid_controller.cpp
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_hybrid_controller.cpp 2009-04-15 20:22:53 UTC (rev 13882)
+++ pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_hybrid_controller.cpp 2009-04-15 20:22:53 UTC (rev 13883)
@@ -223,14 +223,6 @@
// Computes the desired wrench from the command
- // Computes the rotational error
- KDL::Vector rot_error =
- diff(KDL::Rotation::RPY(
- mode_[3] == robot_msgs::TaskFrameFormalism::POSITION ? setpoint_[3] : 0.0,
- mode_[4] == robot_msgs::TaskFrameFormalism::POSITION ? setpoint_[4] : 0.0,
- mode_[5] == robot_msgs::TaskFrameFormalism::POSITION ? setpoint_[5] : 0.0),
- tool.M.R);
-
// Computes the filtered twist
if (use_filter_)
{
@@ -246,92 +238,74 @@
twist_meas_filtered_ = twist_meas_;
}
+ // Computes the rotational error
+ KDL::Vector rot_error =
+ diff(KDL::Rotation::RPY(
+ mode_[3] == robot_msgs::TaskFrameFormalism::POSITION ? setpoint_[3] : 0.0,
+ mode_[4] == robot_msgs::TaskFrameFormalism::POSITION ? setpoint_[4] : 0.0,
+ mode_[5] == robot_msgs::TaskFrameFormalism::POSITION ? setpoint_[5] : 0.0),
+ tool.M.R);
+
+ // Computes the desired pose
for (int i = 0; i < 6; ++i)
{
- twist_desi_[i] = 0;
- wrench_desi_[i] = 0;
- pose_desi_[i] = 0;
+ if (mode_[i] == robot_msgs::TaskFrameFormalism::POSITION)
+ pose_desi_[i] = setpoint_[i];
+ else
+ pose_desi_[i] = 0.0;
+ }
- switch(mode_[i])
+ // Computes the desired twist
+ for (int i = 0; i < 6; ++i)
+ {
+ switch (mode_[i])
{
case robot_msgs::TaskFrameFormalism::POSITION:
- pose_desi_[i] = setpoint_[i];
if (i < 3) { // Translational position
- wrench_desi_[i] = pose_pids_[i].updatePid(tool.p.p[i] - setpoint_[i], twist_meas_filtered_[i], dt);
+ twist_desi_[i] = pose_pids_[i].updatePid(tool.p.p[i] - pose_desi_[i], twist_meas_filtered_[i], dt);
}
else { // Rotational position
- wrench_desi_[i] = pose_pids_[i].updatePid(rot_error[i - 3], twist_meas_filtered_[i], dt);
+ twist_desi_[i] = pose_pids_[i].updatePid(rot_error[i - 3], twist_meas_filtered_[i], dt);
}
-
break;
case robot_msgs::TaskFrameFormalism::VELOCITY:
twist_desi_[i] = setpoint_[i];
- wrench_desi_[i] = twist_pids_[i].updatePid(tool.GetTwist()[i] - setpoint_[i], dt);
break;
- case robot_msgs::TaskFrameFormalism::FORCE:
- wrench_desi_[i] = setpoint_[i];
- break;
- default:
- abort();
}
}
- static realtime_tools::RealtimePublisher<std_msgs::Float64MultiArray> debug("/d", 2);
- bool debug_locked = debug.trylock();
- if (debug_locked)
- debug.msg_.data.clear();
-#if 0
- // Velocity saturation
+ // Limits the velocity
if (saturated_velocity_ >= 0.0)
{
- KDL::Vector &v = twist_meas_filtered_.vel;
-
- if (v.Norm() > saturated_velocity_)
+ if (twist_desi_.vel.Norm() > saturated_velocity_)
{
- double restoring = -k_saturated_velocity_ * (v.Norm() - saturated_velocity_);
- wrench_desi_.force += restoring * (v / v.Norm());
- if (debug_locked) {
- debug.msg_.data.push_back(v.Norm() - saturated_velocity_);
- debug.msg_.data.push_back(restoring);
- }
+ twist_desi_.vel = saturated_velocity_ * twist_desi_.vel / twist_desi_.vel.Norm();
}
- else if (debug_locked) {
- debug.msg_.data.push_back(0);
- debug.msg_.data.push_back(0);
- }
-
-
}
-#else
- // Velocity saturation
- if (saturated_velocity_ >= 0.0)
+ // Computes the desired wrench
+ for (int i = 0; i < 6; ++i)
{
- KDL::Vector &v = twist_meas_filtered_.vel;
-
- // Desired force along the direction of the current velocity
- double fv = dot(wrench_desi_.force, twist_meas_filtered_.vel) / v.Norm();
-
- // Max allowed force along the current velocity direction
- double fv_max = -k_saturated_velocity_ * (v.Norm() - saturated_velocity_);
-
- if (debug_locked) {
- debug.msg_.data.push_back(v.Norm());
- debug.msg_.data.push_back(v.Norm() - saturated_velocity_);
- debug.msg_.data.push_back(fv);
- debug.msg_.data.push_back(fv_max);
-
- KDL::Vector diff = (v / v.Norm()) * (fv - fv_max);
- debug.msg_.data.push_back(diff.x());
- debug.msg_.data.push_back(diff.y());
- debug.msg_.data.push_back(diff.z());
+ switch (mode_[i])
+ {
+ case robot_msgs::TaskFrameFormalism::POSITION:
+ case robot_msgs::TaskFrameFormalism::VELOCITY:
+ wrench_desi_[i] = twist_pids_[i].updatePid(twist_meas_filtered_[i] - twist_desi_[i], dt);
+ break;
+ case robot_msgs::TaskFrameFormalism::FORCE:
+ wrench_desi_[i] = setpoint_[i];
+ break;
+ default:
+ abort();
}
- if (fv > fv_max)
- debug.msg_.data.push_back(99);
- wrench_desi_.force -= (v / v.Norm()) * (fv - fv_max);
}
-#endif
+
+ static realtime_tools::RealtimePublisher<std_msgs::Float64MultiArray> debug("/d", 2);
+ bool debug_locked = debug.trylock();
if (debug_locked)
+ debug.msg_.data.clear();
+
+ if (debug_locked)
debug.unlockAndPublish();
// Transforms the wrench from the task frame to the chain root frame
@@ -381,10 +355,6 @@
setpoint_[i] = frame.p.p[i];
}
frame.M.R.GetRPY(setpoint_[3], setpoint_[4], setpoint_[5]);
-
- ROS_INFO("Starting pose: (%.2lf, %.2lf, %.2lf) (%.3lf, %.3lf, %.3lf)",
- setpoint_[0], setpoint_[1], setpoint_[2],
- setpoint_[3], setpoint_[4], setpoint_[5]);
break;
}
case robot_msgs::TaskFrameFormalism::VELOCITY:
Modified: pkg/trunk/robot_descriptions/pr2/pr2_default_controllers/gains/arm_pose.yaml
===================================================================
--- pkg/trunk/robot_descriptions/pr2/pr2_default_controllers/gains/arm_pose.yaml 2009-04-15 20:22:53 UTC (rev 13882)
+++ pkg/trunk/robot_descriptions/pr2/pr2_default_controllers/gains/arm_pose.yaml 2009-04-15 20:22:53 UTC (rev 13883)
@@ -1,16 +1,13 @@
fb_trans:
- p: 1500
- i: 2800
- d: 2.0
- i_clamp: 40.0
- old_p: 1200.0
- old_i: 1000.0
- old_d: 3.0
- old_i_clamp: 3.0
+ p: 60
+ old_p: 20
+ i: 80
+ d: 0
+ i_clamp: 0.2
fb_rot:
- p: 21.0
- i: 1.0
- d: 0.3
+ p: 10
+ i: 0
+ d: 0
i_clamp: 0.4
ff_trans: 0.0
ff_rot: 0.0
Modified: pkg/trunk/robot_descriptions/pr2/pr2_default_controllers/gains/arm_twist.yaml
===================================================================
--- pkg/trunk/robot_descriptions/pr2/pr2_default_controllers/gains/arm_twist.yaml 2009-04-15 20:22:53 UTC (rev 13882)
+++ pkg/trunk/robot_descriptions/pr2/pr2_default_controllers/gains/arm_twist.yaml 2009-04-15 20:22:53 UTC (rev 13883)
@@ -1,11 +1,11 @@
fb_trans:
- p: 20.0
- i: 0.5
+ p: 30.0
+ i: 0.0
d: 0.0
i_clamp: 1.0
fb_rot:
p: 1.5
- i: 0.1
+ i: 0.0
d: 0.0
i_clamp: 0.2
ff_trans: 20.0
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
