Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[9954] pkg/trunk
|
From: <stu...@us...> - 2009-01-22 02:16:52
|
Revision: 9954
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=9954&view=rev
Author: stuglaser
Date: 2009-01-22 02:16:46 +0000 (Thu, 22 Jan 2009)
Log Message:
-----------
Changed constraint controller to include the torso link and to ignore fixed joints.
Modified Paths:
--------------
pkg/trunk/controllers/robot_mechanism_controllers/src/endeffector_constraint_controller.cpp
pkg/trunk/demos/arm_gazebo/r_arm_constraint.launch
pkg/trunk/demos/arm_gazebo/r_arm_constraint_controller.xml
pkg/trunk/robot_descriptions/pr2/pr2_defs/robots/r_arm_groups.xml
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-22 01:54:15 UTC (rev 9953)
+++ pkg/trunk/controllers/robot_mechanism_controllers/src/endeffector_constraint_controller.cpp 2009-01-22 02:16:46 UTC (rev 9954)
@@ -88,10 +88,10 @@
// some parameters
node->param("constraint/wall_x" , wall_x , 0.6) ; /// location of the wall
- node->param("constraint/threshold_x" , threshold_x , 0.2 ) ; /// distance within the wall to apply constraint force
+ node->param("constraint/threshold_x" , threshold_x , 0.05 ) ; /// distance within the wall to apply constraint force
node->param("constraint/wall_r" , wall_r , 0.2 ) ; /// cylinder radius
node->param("constraint/threshold_r" , threshold_r , 0.05) ; /// radius over with constraint is applied
- node->param("constraint/f_x_max" , f_x_max , -20.0) ; /// max x force
+ node->param("constraint/f_x_max" , f_x_max , -10000.0) ; /// max x force
node->param("constraint/f_y_max" , f_r_max , -20.0) ; /// max r force
@@ -174,8 +174,13 @@
// get the joint positions
JntArray jnt_pos(num_joints_);
+ unsigned int j = 0;
for (unsigned int i=0; i<num_joints_; i++)
- jnt_pos(i) = joints_[i]->position_;
+ {
+ while (joints_[j]->joint_->type_ == mechanism::JOINT_FIXED)
+ ++j;
+ jnt_pos(i) = joints_[j++]->position_;
+ }
// get the chain jacobian
Jacobian jacobian(num_joints_, num_segments_);
@@ -189,6 +194,7 @@
constraint_wrench_ = constraint_jac_ * constraint_force_;
// convert the wrench into joint torques
+ j = 0;
JntArray constraint_torq(num_joints_);
for (unsigned int i=0; i<num_joints_; i++)
{
@@ -197,7 +203,10 @@
{
constraint_torq(i) += (jacobian(j,i) * constraint_wrench_(j));
}
- joints_[i]->commanded_effort_ = constraint_torq(i);
+
+ while (joints_[j]->joint_->type_ == mechanism::JOINT_FIXED)
+ ++j;
+ joints_[j++]->commanded_effort_ = constraint_torq(i);
}
}
@@ -220,7 +229,8 @@
//ROS_ERROR("x_dist_to_wall: %f m\n", (x_dist_to_wall));
if (x_dist_to_wall > 0)
{
- f_x = (exp(x_dist_to_wall)-1) * f_x_max; /// @todo: FIXME, replace with some exponential function
+ //f_x = (exp(x_dist_to_wall)-1) * f_x_max; /// @todo: FIXME, replace with some exponential function
+ f_x = x_dist_to_wall * f_x_max;
//ROS_ERROR("x_dist_to_wall: %f m f_x: %f N\n", x_dist_to_wall, f_x);
if((x_dist_to_wall-threshold_x) > 0)
{
Modified: pkg/trunk/demos/arm_gazebo/r_arm_constraint.launch
===================================================================
--- pkg/trunk/demos/arm_gazebo/r_arm_constraint.launch 2009-01-22 01:54:15 UTC (rev 9953)
+++ pkg/trunk/demos/arm_gazebo/r_arm_constraint.launch 2009-01-22 02:16:46 UTC (rev 9954)
@@ -8,6 +8,7 @@
-->
<!-- send pr2_r_arm.xml to param server -->
+<!--
<param name="robotdesc/pr2" command="$(find xacro)/xacro.py '$(find pr2_defs)/robots/r_arm.xacro.xml'" />
<!-- -g flag runs gazebo in gui-less mode -->
@@ -22,8 +23,9 @@
<node pkg="gazebo_plugin" type="urdf2factory" args="robotdesc/pr2" respawn="false" output="screen"/>
+-->
<!-- start arm controller -->
- <node pkg="mechanism_control" type="mech.py" args="sp $(find arm_gazebo)/r_arm_constraint_controller.xml" respawn="false" />
+ <node pkg="mechanism_control" type="spawner.py" args="$(find arm_gazebo)/r_arm_constraint_controller.xml" respawn="false" />
</group>
Modified: pkg/trunk/demos/arm_gazebo/r_arm_constraint_controller.xml
===================================================================
--- pkg/trunk/demos/arm_gazebo/r_arm_constraint_controller.xml 2009-01-22 01:54:15 UTC (rev 9953)
+++ pkg/trunk/demos/arm_gazebo/r_arm_constraint_controller.xml 2009-01-22 02:16:46 UTC (rev 9954)
@@ -1,6 +1,6 @@
<controllers>
<controller type="EndeffectorConstraintControllerNode" name="arm_constraint" topic="arm_constraint">
- <chain root="r_shoulder_pan_link" tip="r_wrist_roll_link" offset="0 0 0" />
+ <chain root="torso_lift_link" tip="r_wrist_roll_link" offset="0 0 0" />
</controller>
</controllers>
Modified: pkg/trunk/robot_descriptions/pr2/pr2_defs/robots/r_arm_groups.xml
===================================================================
--- pkg/trunk/robot_descriptions/pr2/pr2_defs/robots/r_arm_groups.xml 2009-01-22 01:54:15 UTC (rev 9953)
+++ pkg/trunk/robot_descriptions/pr2/pr2_defs/robots/r_arm_groups.xml 2009-01-22 02:16:46 UTC (rev 9954)
@@ -1,8 +1,9 @@
<?xml version="1.0"?>
<robot name="pr2"><!-- name of the robot-->
-
+
<group name="right_arm" flags="planning kinematic">
+ torso_lift_link
r_shoulder_pan_link
r_shoulder_lift_link
r_upper_arm_roll_link
@@ -14,10 +15,10 @@
<map name="RRT" flag="planning">
<elem key="range" value="0.75" />
</map>
-
+
<map name="LazyRRT" flag="planning">
<elem key="range" value="0.75" />
</map>
</group>
-
+
</robot>
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
