Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[9035] pkg/trunk
|
From: <hsu...@us...> - 2009-01-08 04:18:32
|
Revision: 9035
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=9035&view=rev
Author: hsujohnhsu
Date: 2009-01-08 04:18:27 +0000 (Thu, 08 Jan 2009)
Log Message:
-----------
arm constraint controller working in sim.
Modified Paths:
--------------
pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/endeffector_constraint_controller.h
pkg/trunk/controllers/robot_mechanism_controllers/src/endeffector_constraint_controller.cpp
Added Paths:
-----------
pkg/trunk/demos/arm_gazebo/r_arm_constraint.launch
pkg/trunk/demos/arm_gazebo/r_arm_constraint_controller.xml
Modified: pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/endeffector_constraint_controller.h
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/endeffector_constraint_controller.h 2009-01-08 03:54:17 UTC (rev 9034)
+++ pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/endeffector_constraint_controller.h 2009-01-08 04:18:27 UTC (rev 9035)
@@ -47,6 +47,7 @@
#include "Eigen/SVD"
#include "Eigen/Core"
#include "robot_kinematics/robot_kinematics.h"
+#include <std_msgs/VisualizationMarker.h>
namespace controller {
@@ -76,6 +77,16 @@
Eigen::Matrix<float,6,6> constraint_jac_;
Eigen::Matrix<float,6,1> constraint_wrench_;
KDL::Frame endeffector_frame_;
+
+ // some parameters to define the constraint
+ double wall_x; /// @todo: hardcoded x wall location
+ double threshold_x; //@todo: hardcoded wall threshold, activate constraint force if closer than this
+ double wall_r; /// @todo: hardcoded wall_radius
+ double threshold_r; //@todo: hardcoded wall threshold, activate constraint force if closer than this
+ double f_x_max;
+ double f_y_max;
+ double f_z_max;
+
};
Modified: pkg/trunk/controllers/robot_mechanism_controllers/src/endeffector_constraint_controller.cpp
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/src/endeffector_constraint_controller.cpp 2009-01-08 03:54:17 UTC (rev 9034)
+++ pkg/trunk/controllers/robot_mechanism_controllers/src/endeffector_constraint_controller.cpp 2009-01-08 04:18:27 UTC (rev 9035)
@@ -84,6 +84,15 @@
if (serial_chain == NULL)
fprintf(stderr, "Got NULL Chain\n") ;
+ // some parameters
+ node->param("constraint/wall_x", wall_x, 0.75) ;
+ node->param("constraint/threshold_x", threshold_x, 0.2) ;
+ node->param("constraint/wall_r", wall_r, 0.2) ;
+ node->param("constraint/threshold_r", threshold_r, 0.1) ;
+ node->param("constraint/f_x_max", f_x_max, 20.0) ;
+ node->param("constraint/f_y_max", f_y_max, 20.0) ;
+ node->param("constraint/f_z_max", f_z_max, 20.0) ;
+
// convert description to KDL chain
chain_ = serial_chain->chain;
num_joints_ = chain_.getNrOfJoints();
@@ -183,8 +192,8 @@
jnt_torq(i) = 0;
for (unsigned int j=0; j<6; j++)
{
- //jnt_torq(i) += (jacobian(j,i) * wrench_desi_(j));
jnt_torq(i) += (jacobian(j,i) * constraint_wrench_(j));
+ jnt_torq(i) += (jacobian(j,i) * wrench_desi_(j)); /// todo: project into null space first before adding
}
joints_[i]->commanded_effort_ = jnt_torq(i);
}
@@ -207,7 +216,7 @@
double df_dx = -1.0; // we are describing a wall at constant x, x- side is allowed
double df_dy = -cos(ee_theta); // radial lines toward origin
- double df_dz = sin(ee_theta); // radial lines toward origin
+ double df_dz = -sin(ee_theta); // radial lines toward origin
// Constraint Jacobian (normals to the constraint surface)
constraint_jac_(0,0)= df_dx;
@@ -221,42 +230,53 @@
////////////////////////////////////////////
// x-direction force is a function of endeffector distance from the wall
// @todo: FIXME: hardcoded wall at x=1
- double x_wall = 1.0; /// @todo: hardcoded x wall location
- double x_distance = x_wall - endeffector_frame_.p(0);
- double x_threshold = 0.2; //@todo: hardcoded wall threshold, activate constraint force if closer than this
- double f_x = 0;
- double f_x_scale = 10; //@todo: f_x_scale * exp(distance) = force applied in x direction
+ double x_distance = wall_x - endeffector_frame_.p(0);
+ double f_x;
// assign x-direction constraint force f_x if within range of the wall
- if (x_distance >0 && x_distance < x_threshold)
+ if (x_distance >0 && x_distance < threshold_x)
{
- f_x = exp(x_distance) * f_x_scale * df_dx; /// @todo: FIXME, some exponential function
+ f_x = x_distance/threshold_x * f_x_max; /// @todo: FIXME, replace with some exponential function
}
else if (x_distance <= 0)
+ {
+ f_x = f_x_max;
ROS_ERROR("wall x breach! by: %f m\n",x_distance);
+ }
+ else
+ {
+ f_x = 0;
+ }
/// yz-force magnitude is a function of endeffector distance from unit circle
double ee_radius = sqrt( endeffector_frame_.p(1)*endeffector_frame_.p(1) + endeffector_frame_.p(2)*endeffector_frame_.p(2) );
- double wall_radius = 1.0; /// @todo: hardcoded wall_radius
- double r_distance = wall_radius - ee_radius;
- double r_threshold = 0.2; //@todo: hardcoded wall threshold, activate constraint force if closer than this
- double f_y_scale = 10; //@todo: f_y_scale * exp(distance) = force applied in y direction
- double f_z_scale = 10; //@todo: f_z_scale * exp(distance) = force applied in z direction
- double f_y = 0;
- double f_z = 0;
+ double r_distance = wall_r - ee_radius;
+ double f_y, f_z;
// assign x-direction constraint force f_x if within range of the wall
- if (r_distance > 0 && r_distance < r_threshold)
+ if (r_distance > 0 && r_distance < threshold_r)
{
- f_y = exp(r_distance) * f_y_scale * df_dy; /// @todo: FIXME, some exponential function
- f_z = exp(r_distance) * f_z_scale * df_dz; /// @todo: FIXME, some exponential function
+ f_y = r_distance/threshold_r * f_y_max; /// @todo: FIXME, replace with some exponential function
+ f_z = r_distance/threshold_r * f_z_max; /// @todo: FIXME, replace with some exponential function
}
else if (r_distance <= 0)
+ {
+ f_y = f_y_max; /// @todo: FIXME, some exponential function
+ f_z = f_z_max; /// @todo: FIXME, some exponential function
ROS_ERROR("wall radius breach! by: %f m\n",r_distance);
+ }
+ else
+ {
+ f_y = 0;
+ f_z = 0;
+ }
constraint_wrench_(0) = f_x;
constraint_wrench_(1) = f_y;
constraint_wrench_(2) = f_z;
+
+ ROS_WARN("force magnitude (%f, %f, %f)\n",f_x,f_y,f_z);
+
}
@@ -291,6 +311,30 @@
&EndeffectorConstraintControllerNode::command, this, 1);
guard_command_.set(topic_ + "/command");
+ node->advertise<std_msgs::VisualizationMarker>( "visualizationMarker", 0 );
+
+ // visualization not working yet
+ std_msgs::VisualizationMarker marker;
+ marker.header.frame_id = "base_link";
+ marker.id = 0;
+ marker.type = 2;
+ marker.action = 0;
+ marker.x = 0.7;
+ marker.y = 0;
+ marker.z = 0;
+ marker.yaw = 0;
+ marker.pitch = 0;
+ marker.roll = 0.0;
+ marker.xScale = 0.01;
+ marker.yScale = 0.3;
+ marker.zScale = 0.3;
+ marker.alpha = 100;
+ marker.r = 0;
+ marker.g = 255;
+ marker.b = 0;
+ node->publish("visualizationMarker", marker );
+
+
return true;
}
Added: pkg/trunk/demos/arm_gazebo/r_arm_constraint.launch
===================================================================
--- pkg/trunk/demos/arm_gazebo/r_arm_constraint.launch (rev 0)
+++ pkg/trunk/demos/arm_gazebo/r_arm_constraint.launch 2009-01-08 04:18:27 UTC (rev 9035)
@@ -0,0 +1,39 @@
+<launch>
+ <group name="wg">
+ <!-- send pr2_r_arm.xml to param server -->
+ <param name="robotdesc/pr2" command="$(find xacro)/xacro.py '$(find wg_robot_description)/pr2_arm_test/pr2_r_arm.xacro.xml'" />
+
+ <!-- -g flag runs gazebo in gui-less mode -->
+ <node pkg="gazebo" type="gazebo" args="-n $(find gazebo_robot_description)/gazebo_worlds/empty.world" respawn="false" output="screen">
+ <env name="LD_LIBRARY_PATH" value="$(find gazebo_plugin)/lib:$(find gazebo)/gazebo/lib:$(find Cg)/Cg/lib:$(find boost)/boost/lib" />
+ <env name="GAZEBO_RESOURCE_PATH" value="$(find gazebo_robot_description)/world" />
+ <env name="OGRE_RESOURCE_PATH" value="$(find ogre)/ogre/lib/OGRE" />
+ <env name="MC_RESOURCE_PATH" value="$(find gazebo_robot_description)/world" />
+ </node>
+
+ <!-- push robotdesc/pr2 to factory and spawn robot in gazebo -->
+ <node pkg="gazebo_plugin" type="urdf2factory" args="robotdesc/pr2" respawn="false" output="screen"/>
+
+
+ <!-- start arm controller -->
+ <!--
+ <node pkg="ogre_visualizer" type="standalone_visualizer.py" respawn="false" output="screen" />
+ <node pkg="mechanism_control" type="mech.py" args="sp $(find arm_gazebo)/r_arm_default_controller.xml" respawn="false" />
+ <node pkg="mechanism_control" type="mech.py" args="sp $(find arm_gazebo)/r_arm_cartesian_pos_controller.xml" respawn="false" />
+ <node pkg="mechanism_control" type="mech.py" args="sp $(find arm_gazebo)/r_arm_trajectory_controller.xml" respawn="false" />
+ -->
+ <node pkg="mechanism_control" type="mech.py" args="sp $(find arm_gazebo)/r_arm_constraint_controller.xml" respawn="false" />
+
+ <!-- send arm a command -->
+ <!-- open gripper .5 radians
+ <node pkg="robot_mechanism_controllers" type="control.py" args="set r_gripper_controller 0.5" respawn="false" output="screen" />
+ -->
+ <!-- tests
+ <node pkg="robot_mechanism_controllers" type="control.py" args="set r_gripper_controller 0.5" respawn="false" output="screen" />
+ -->
+
+ <!-- for visualization -->
+ <!-- node pkg="pr2_gui" type="pr2_gui" respawn="false" output="screen" / -->
+ </group>
+</launch>
+
Added: pkg/trunk/demos/arm_gazebo/r_arm_constraint_controller.xml
===================================================================
--- pkg/trunk/demos/arm_gazebo/r_arm_constraint_controller.xml (rev 0)
+++ pkg/trunk/demos/arm_gazebo/r_arm_constraint_controller.xml 2009-01-08 04:18:27 UTC (rev 9035)
@@ -0,0 +1,5 @@
+<controllers>
+ <controller type="EndeffectorConstraintControllerNode" name="arm_constraint" topic="arm_constraint">
+ <chain root="base_link" tip="r_wrist_roll_link" offset="0.0 0 0" />
+ </controller>
+</controllers>
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
