Working wam in 4/7/7h mode. NOTE: had to adjust motor constants to ge…

…t base yaw to work.

config/calibration_data/wam4/gravitycal.conf

@@ -0,0 +1,9 @@
+gravity_compensation : 
+{
+  mus = (
+  			( 0.2667890151, 3.377309876e-20, 5.798423849e-05 ),
+  			( 0.008582424569, 0.02793043314, 1.373751226 ),
+  			( -0.04104413786, 0.1237016607, 0.04589455098 ),
+  			( -0.01694411736, -0.0001243921853, 0.1786010977 )
+  		);
+};

config/calibration_data/wam4/zerocal.conf

@@ -0,0 +1 @@
+home = ( 0, -2, 0, 3.13 );

config/calibration_data/wam7w/gravitycal.conf

@@ -0,0 +1,12 @@
+gravity_compensation : 
+{
+  mus = (
+  			( -0.4619813284, -8.777142616e-19, -0.0001650712715 ),
+  			( 0.09324682625, -0.1429915845, 2.3827622 ),
+  			( 0.09102604107, 0.1774000857, 0.1094180014 ),
+  			( -0.1263857613, -0.05701896802, 0.5415280943 ),
+  			( 0.002226522425, 0.01362529161, -0.00101195702 ),
+  			( -0.0008998072939, -0.00499246526, 0.02592270061 ),
+  			( -3.960032732e-05, -7.543236065e-05, 1.303052607e-05 )
+  		);
+};

config/calibration_data/wam7w/zerocal.conf

@@ -0,0 +1 @@
+home = ( 0, -2, 0, 3.13, 0, 0, 0 );

config/libbarrett.conf

@@ -0,0 +1,43 @@
+bus:
+{
+	port = 0;
+};
+
+
+wam4:
+{
+	low_level:
+	{
+		# Home position and zero-angle calibration data
+		@include "calibration_data/wam4/zerocal.conf"
+
+		j2mp = (( -38.0,      0,        0,     0 ),
+				(     0,  28.25, -16.8155,     0 ),
+				(     0, -28.25, -16.8155,     0 ),
+				(     0,      0,        0, -18.0 ));
+		joint_encoder_counts = (1578399, 655360, 655360, 327680);
+	};
+};
+
+
+wam7w:
+{
+	low_level:
+	{
+		# Home position and zero-angle calibration data
+		@include "calibration_data/wam7w/zerocal.conf"
+
+		j2mp = (( -38.0,      0,        0,     0,   0,    0,      0 ),
+				(     0,  28.25, -16.8155,     0,   0,    0,      0 ),
+				(     0, -28.25, -16.8155,     0,   0,    0,      0 ),
+				(     0,      0,        0, -18.0,   0,    0,      0 ),
+				(     0,      0,        0,     0, 9.7, -9.7,      0 ),
+				(     0,      0,        0,     0, 9.7,  9.7,      0 ),
+				(     0,      0,        0,     0,   0,    0, -14.93 ));
+		joint_encoder_counts = (1578399, 655360, 655360, 327680, 0, 0, 0);
+	};
+};
+
+# The CONFIG_VERSION number is used during the upgrade process to detect
+# out-of-date configuration files. Please do not change it.
+CONFIG_VERSION = 1;

idle.launch

@@ -32,7 +32,7 @@
       man_pid:
         root_link: wam/base_link
         tip_link: wam/wrist_palm_link
-        p_gains: [200.0, 200.0, 200.0, 200.0, 40.0, 40.0, 20.0]
+        p_gains: [400.0, 350.0, 250.0, 200.0, 40.0, 40.0, 10.0]
         d_gains: [3.0, 3.0, 2.0, 2.0, 0.5, 0.5, 0.08]
       man_traj:
         root_link: wam/base_link

idle.ops

@@ -116,6 +116,7 @@ inverse_dynamics.configure();
 scheme.enableBlock("devices",false);
 scheme.disableBlock("calibration");
 scheme.enableBlock("manual",false);
+scheme.enableBlock("inverse_dynamics",false);
 
 scheme.start();
 

ops/barrett_manager.ops

@@ -0,0 +1,20 @@
+
+import("rtt_ros");
+
+/* Create the barrett manager */
+ros.import("oro_barrett_hw");
+loadComponent("barrett_manager","oro_barrett_hw::BarrettHWManager");
+loadService("barrett_manager","rosparam");
+
+barrett_manager.rosparam.getAll();
+barrett_manager.rosparam.getAbsolute("robot_description");
+barrett_manager.configure();
+
+/* Configure conman interface */
+/* Slave the barrett manager's activity to the scheme */
+addPeer("scheme","barrett_manager");
+scheme.addGroup("devices");
+scheme.addToGroup("barrett_manager","devices");
+scheme.latchInputs("barrett_manager",true);
+//barrett_manager.conman_hook.setDesiredMinPeriod(0.0);
+

ops/comp_calibration.ops

@@ -0,0 +1,35 @@
+
+ros.import("lcsr_controllers");
+
+/* Create calibration PID controller loop */
+loadComponent("cal_pid","lcsr_controllers::JointPIDController");
+loadComponent("cal_traj","lcsr_controllers::JointTrajGeneratorKDL");
+loadComponent("cal_sac","lcsr_controllers::SemiAbsoluteCalibrationController");
+
+connect("barrett_manager.wam.position_out", "cal_sac.joint_position_in", ConnPolicy());
+connect("barrett_manager.wam.resolver_offset_out", "cal_sac.resolver_offset_in", ConnPolicy());
+
+connect("cal_sac.joint_position_estimate_out", "cal_pid.joint_position_in", ConnPolicy());
+connect("barrett_manager.wam.velocity_out", "cal_pid.joint_velocity_in", ConnPolicy());
+connect("cal_traj.joint_position_out", "cal_pid.joint_position_cmd_in", ConnPolicy());
+connect("cal_traj.joint_velocity_out", "cal_pid.joint_velocity_cmd_in", ConnPolicy());
+
+connect("cal_sac.joint_position_estimate_out", "cal_traj.joint_position_in", ConnPolicy());
+connect("cal_sac.joint_position_desired_out", "cal_traj.joint_position_cmd_in", ConnPolicy());
+
+connect("cal_pid.joint_effort_out", "barrett_manager.wam.effort_in", ConnPolicy());
+
+cal_pid.configure();
+cal_sac.configure();
+cal_traj.configure();
+
+/* Configure conman interface */
+addPeer("scheme","cal_pid");
+addPeer("scheme","cal_traj");
+addPeer("scheme","cal_sac");
+
+scheme.addGroup("calibration");
+scheme.addToGroup("cal_pid","calibration");
+scheme.addToGroup("cal_traj","calibration");
+scheme.addToGroup("cal_sac","calibration");
+

ops/comp_manual.ops

@@ -0,0 +1,46 @@
+
+ros.import("conman_blocks");
+ros.import("lcsr_controllers");
+
+/* Create Feed-Forward inverse dynamics component */
+loadComponent("inverse_dynamics","lcsr_controllers::IDControllerKDL");
+connect("barrett_manager.wam.position_out", "inverse_dynamics.joint_position_in", ConnPolicy());
+connect("barrett_manager.wam.velocity_out", "inverse_dynamics.joint_velocity_in", ConnPolicy());
+
+/* Create manual joint-space PID controller loop */
+loadComponent("man_pid","lcsr_controllers::JointPIDController");
+loadComponent("man_traj","lcsr_controllers::JointTrajGeneratorKDL");
+loadComponent("wam_effort_sum","conman_blocks::FeedForwardFeedBack");
+
+connect("barrett_manager.wam.position_out", "man_pid.joint_position_in", ConnPolicy());
+connect("barrett_manager.wam.velocity_out", "man_pid.joint_velocity_in", ConnPolicy());
+connect("man_traj.joint_position_out", "man_pid.joint_position_cmd_in", ConnPolicy());
+connect("man_traj.joint_velocity_out", "man_pid.joint_velocity_cmd_in", ConnPolicy());
+
+connect("barrett_manager.wam.position_out", "man_traj.joint_position_in", ConnPolicy());
+
+connect("man_pid.joint_effort_out", "wam_effort_sum.feedback_in", ConnPolicy());
+connect("inverse_dynamics.joint_effort_out", "wam_effort_sum.feedforward_in", ConnPolicy());
+
+connect("wam_effort_sum.sum_out", "barrett_manager.wam.effort_in", ConnPolicy());
+
+man_pid.configure();
+man_traj.configure();
+wam_effort_sum.configure();
+inverse_dynamics.configure();
+
+/* Configure conman interface */
+addPeer("scheme","man_pid");
+addPeer("scheme","man_traj");
+addPeer("scheme","wam_effort_sum");
+addPeer("scheme","inverse_dynamics");
+
+/* Create manual group  */
+scheme.addGroup("manual");
+scheme.addToGroup("man_pid","manual");
+scheme.addToGroup("man_traj","manual");
+scheme.addToGroup("wam_effort_sum","manual");
+
+/* Add ID controller */
+scheme.addBlock("inverse_dynamics");
+

ops/conman.ops

@@ -0,0 +1,13 @@
+
+ros.import("conman");
+ros.import("conman_ros");
+
+/* Create the conman scheme */
+loadComponent("scheme","conman::Scheme");
+setActivity("scheme",0.001,HighestPriority,ORO_SCHED_RT)
+loadService("scheme","conman_ros")
+
+/* Start the Scheme */
+scheme.configure();
+
+scheme.start();

ops/hand.ops

@@ -0,0 +1,10 @@
+
+/* Create a BHand */
+barrett_manager.configureHand("wam/bhand");
+
+/* Publish joint state to ROS */
+stream("barrett_manager.hand.joint_state_out",rostopic.connection("hand_joint_states"));
+
+/* Subscribe to hand command from ROS */
+stream("barrett_manager.hand.joint_cmd_in",rostopic.connection("hand_cmd"));
+

ops/test.ops

@@ -0,0 +1 @@
+"a" + "b"

ops/wam4.ops

@@ -0,0 +1,10 @@
+
+/* Create a 7-DOF WAM */
+barrett_manager.configureWam4("wam");
+barrett_manager.rosparam.getComponentPrivate("wam");
+
+/* Configure conman interface */
+barrett_manager.conman_hook.setInputExclusivity("wam.effort_in",1)
+
+/* Publish joint state to ROS */
+stream("barrett_manager.wam.joint_state_out",rostopic.connection("wam_joint_states"));

ops/wam7.ops

@@ -0,0 +1,10 @@
+
+/* Create a 7-DOF WAM */
+barrett_manager.configureWam7("wam");
+barrett_manager.rosparam.getComponentPrivate("wam");
+
+/* Configure conman interface */
+barrett_manager.conman_hook.setInputExclusivity("wam.effort_in",1)
+
+/* Publish joint state to ROS */
+stream("barrett_manager.wam.joint_state_out",rostopic.connection("wam_joint_states"));

wam4.launch

@@ -0,0 +1,50 @@
+<launch>
+
+  <arg name="LOG_LEVEL" default="info"/>
+  <arg name="DEBUG" default="false"/>
+
+  <include file="$(find rtt_ros)/launch/deployer.launch">
+    <arg name="NAME" value="barrett"/>
+    <arg name="DEPLOYER_ARGS" value="-s $(find lcsr_barrett)/wam4.ops"/>
+    <arg name="LOG_LEVEL" value="debug"/>
+    <arg name="DEBUG" value="$(arg DEBUG)"/>
+  </include>
+
+  <group ns="barrett">
+    <param name="barrett_manager/config_path" value="$(find lcsr_barrett)/config/libbarrett.conf"/>
+    <rosparam>
+      barrett_manager:
+        bus_id: 0
+        wam:
+          home_position: [0.0, -1.5708, 0.0, 3.1415]
+          home_resolver_offset: [0.544563, -2.09235, 0.944932, -1.35757]
+      wam_effort_sum: { dim: 4 }
+      man_pid:
+        root_link: wam/base_link
+        tip_link: wam/stump_link
+        p_gains: [400.0, 350.0, 250.0, 200.0]
+        d_gains: [3.0, 3.0, 2.0, 2.0]
+      man_traj:
+        root_link: wam/base_link
+        tip_link: wam/stump_link
+        trap_max_vels:      [0.2, 0.4, 1.5, 0.8]
+        trap_max_accs:      [0.2, 0.6, 0.6, 0.4]
+        position_tolerance: [0.1, 0.1, 0.1, 0.1]
+        velocity_tolerance: [0.1, 0.1, 0.1, 0.1]
+      inverse_dynamics:
+        root_link: wam/base_link
+        tip_link: wam/stump_link
+        end_effector_frame_link: wam/wrist_palm_link
+        gravity: [-9.8, 0.0, 0.0]
+    </rosparam>
+  </group>
+
+  <param name="robot_description" command="$(find xacro)/xacro.py
+    '$(find barrett_model)/robots/wam_4dof_wam.urdf.xacro'" />
+
+  <node pkg="robot_state_publisher" type="robot_state_publisher"
+    name="wam_state_publisher">
+    <param name="publish_frequency" value="50"/>
+    <remap from="joint_states" to="wam_joint_states"/>
+  </node>
+</launch>

wam4.ops

@@ -0,0 +1,22 @@
+
+import("rtt_ros");
+import("rtt_rospack");
+
+/* Load conman */
+runScript(rospack.find("lcsr_barrett")+"/ops/conman.ops");
+
+/* Load barrett manager, 4-dof wam */
+runScript(rospack.find("lcsr_barrett")+"/ops/barrett_manager.ops");
+runScript(rospack.find("lcsr_barrett")+"/ops/wam4.ops");
+
+/* Load manual controllers */
+runScript(rospack.find("lcsr_barrett")+"/ops/comp_manual.ops");
+
+/* Set initially running blocks */
+scheme.enableBlock("devices",false);
+scheme.enableBlock("manual",false);
+scheme.enableBlock("inverse_dynamics",false);
+
+/* Calibrate joint positions */
+// barrett_manager.wam.initialize()
+// barrett_manager.wam.run()

wam7.launch

@@ -0,0 +1,61 @@
+<launch>
+
+  <arg name="LOG_LEVEL" default="debug"/>
+  <arg name="DEBUG" default="false"/>
+
+  <include file="$(find rtt_ros)/launch/deployer.launch">
+    <arg name="NAME" value="barrett"/>
+    <arg name="DEPLOYER_ARGS" value="-s $(find lcsr_barrett)/wam7.ops"/>
+    <arg name="LOG_LEVEL" value="debug"/>
+    <arg name="DEBUG" value="$(arg DEBUG)"/>
+  </include>
+
+  <group ns="barrett">
+    <param name="barrett_manager/config_path" value="$(find lcsr_barrett)/config/libbarrett.conf"/>
+    <rosparam>
+      barrett_manager:
+        bus_id: 0
+        wam:
+          home_position: [0.0, -1.5708, 0.0, 3.1415, 0.0, -1.5708, 1.5708]
+          home_resolver_offset: [0.544563, -2.09235, 0.944932, -1.35757, 2.11383, 1.18423, 2.23808]
+      wam_effort_sum: { dim: 7 }
+      cal_pid:
+        root_link: wam/base_link
+        tip_link: wam/wrist_palm_link
+      cal_traj:
+        root_link: wam/base_link
+        tip_link: wam/wrist_palm_link
+        trap_max_vels: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
+        trap_max_accs: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
+      cal_sac:
+        root_link: wam/base_link
+        tip_link: wam/wrist_palm_link
+      man_pid:
+        root_link: wam/base_link
+        tip_link: wam/wrist_palm_link
+        p_gains: [400.0, 350.0, 250.0, 200.0, 40.0, 40.0, 10.0]
+        d_gains: [3.0, 3.0, 2.0, 2.0, 0.5, 0.5, 0.08]
+      man_traj:
+        root_link: wam/base_link
+        tip_link: wam/wrist_palm_link
+        trap_max_vels: [0.2, 0.4, 1.5, 0.8, 4.0, 4.0, 8.0]
+        trap_max_accs: [0.2, 0.6, 0.6, 0.4, 0.6, 0.6, 0.6]
+        position_tolerance: [0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1]
+        velocity_tolerance: [0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1]
+      inverse_dynamics:
+        root_link: wam/base_link
+        tip_link: wam/wrist_palm_link
+        end_effector_frame_link: wam/wrist_palm_link
+        gravity: [-9.8, 0.0, 0.0]
+    </rosparam>
+  </group>
+
+  <param name="robot_description" command="$(find xacro)/xacro.py
+    '$(find barrett_model)/robots/wam_7dof_wam.urdf.xacro'" />
+
+  <node pkg="robot_state_publisher" type="robot_state_publisher"
+    name="wam_state_publisher">
+    <param name="publish_frequency" value="50"/>
+    <remap from="joint_states" to="wam_joint_states"/>
+  </node>
+</launch>

wam7.ops

@@ -0,0 +1,22 @@
+
+import("rtt_ros");
+import("rtt_rospack");
+
+/* Load conman */
+runScript(rospack.find("lcsr_barrett")+"/ops/conman.ops");
+
+/* Load barrett manager, 7-dof wam */
+runScript(rospack.find("lcsr_barrett")+"/ops/barrett_manager.ops");
+runScript(rospack.find("lcsr_barrett")+"/ops/wam7.ops");
+
+/* Load manual controllers */
+runScript(rospack.find("lcsr_barrett")+"/ops/comp_manual.ops");
+
+/* Set initially running blocks */
+scheme.enableBlock("devices",false);
+scheme.enableBlock("manual",false);
+scheme.enableBlock("inverse_dynamics",false);
+
+/* Calibrate joint positions */
+// barrett_manager.wam.initialize()
+// barrett_manager.wam.run()