diff --git a/.gitignore b/.gitignore
new file mode 100644
index 0000000..0d20b64
--- /dev/null
+++ b/.gitignore
@@ -0,0 +1 @@
+*.pyc
diff --git a/README.md b/README.md
index f0b54ae..95d9ed6 100644
--- a/README.md
+++ b/README.md
@@ -55,6 +55,7 @@ sudo apt-get install libtar-dev protobuf-compiler libprotobuf-dev
```bash
echo "export OROCOS_TARGET=xenomai" >> ~/.bashrc
+export XENOMAI_ROOT_DIR=/usr/xenomai
wget http://web.barrett.com/support/WAM_Installer/libconfig-barrett_1.4.5-1_`dpkg --print-architecture`.deb
sudo dpkg -i libconfig-barrett_1.4.5-1_`dpkg --print-architecture`.deb
```
diff --git a/config/wam_40.yaml b/config/wam_40.yaml
index 4e6162d..21b4383 100644
--- a/config/wam_40.yaml
+++ b/config/wam_40.yaml
@@ -1,7 +1,8 @@
# These are the hardware calibrations for WAM#40
# Zerocal
-home_position: [0.0, -1.58153, 0.0, 3.1415, -1.5708, -1.5708, -3.0]
-home_resolver_offset: [-1.5109710760673565, -0.21935925266764755, -0.48166996739609047, -3.1109130378320806, -1.2011, -0.8851069146100148, -2.5065246074051366]
+home_position: [0.0, -1.5708, 0.0, 3.1415, -1.5708, -1.5708, -3.5112]
+home_resolver_offset: [0.3911651009108388, -0.7086991240031661, 0.25003886842535916,
+ -3.0786994412864814, -0.75318456685185, -0.9894176081862387, -2.4911847995262804]
# Velocity filtering
velocity_cutoff: [10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 1.0]
velocity_smoothing_factor: 0.95
diff --git a/launch/sim/orocos_component_params.launch b/launch/sim/orocos_component_params.launch
index 0b3a45e..ca9eb8f 100644
--- a/launch/sim/orocos_component_params.launch
+++ b/launch/sim/orocos_component_params.launch
@@ -12,7 +12,7 @@
robot_description_param: /robot_description
root_link: $(arg tf_prefix)/base_link
end_effector_frame_link: $(arg tf_prefix)/wrist_palm_link
- gravity: [0.0, -9.8, 0.0]
+ gravity: [0.0, -9.81, 0.0]
inertia_mass_rate: 0.3
inertia_com_rate: 0.05
max_attached_mass: 3.0
@@ -58,7 +58,7 @@
p_gains: [350.0, 350.0, 350.0, 300.0, 25.0, 25.0, 2.0]
- d_gains: [30.0, 30.0, 20.0, 10.0, 4.5, 3.5, 0.8]
+ d_gains: [10.0, 20.0, 20.0, 10.0, 4.5, 3.5, 0.8]
position_tolerance: [0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5]
velocity_tolerance: [2.0, 2.0, 2.0, 2.0, 2.0, 2.0, 2.0]
diff --git a/launch/sim/wam.world b/launch/sim/wam.world
index e4503c3..857ed76 100644
--- a/launch/sim/wam.world
+++ b/launch/sim/wam.world
@@ -7,10 +7,47 @@
0 0 0 0
-
- model://ground_plane
+
+
+
+
+
0 0 -1.0 0 0 0
-
+ true
+
+
+
+
+ 0 0 1
+ 100 100
+
+
+
+
+
+ 100
+ 50
+
+
+
+
+
+ false
+
+
+ 0 0 1
+ 100 100
+
+
+
+
+
+
+
+
diff --git a/lua/load_controllers.lua b/lua/load_controllers.lua
index 5ebf8c2..218b115 100644
--- a/lua/load_controllers.lua
+++ b/lua/load_controllers.lua
@@ -81,6 +81,7 @@ function load_controllers(depl, scheme, prefix)
connect(wam, "position_out", ik, "positions_in");
connect( ik, "trajectories_out", traj_rml, "joint_traj_cmd_in");
ik:connectPeers(tf)
+ ik:connectServices(tf)
--[[ Create a cartesian interpolator --]]
cart_servo_name = prefix.."cart_servo"
@@ -91,6 +92,7 @@ function load_controllers(depl, scheme, prefix)
connect(wam, "position_out", cart_servo, "positions_in");
connect( cart_servo, "framevel_out", jtns, "framevel_in");
cart_servo:connectPeers(tf)
+ cart_servo:connectServices(tf)
--[[ Create a coulomb friction compensator --]]
coulomb_name = prefix.."coulomb"
diff --git a/models/manipulation_platform.urdf.xacro b/models/manipulation_platform.urdf.xacro
index b85d240..4f768f3 100644
--- a/models/manipulation_platform.urdf.xacro
+++ b/models/manipulation_platform.urdf.xacro
@@ -3,38 +3,48 @@
-
-
-
-
-
+
+
+
+
+
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
-
- 50000
- 8
-
+
+ 50000
+ 8
+
+
+ 0 0 0 0
+ 0.8 0.8 0.8 0.1
+ 1.0 1.0 1.0 1.0
+
+
+
diff --git a/models/meshes/manipulation_platform.blend b/models/meshes/manipulation_platform.blend
index 62b5032..9930cff 100644
Binary files a/models/meshes/manipulation_platform.blend and b/models/meshes/manipulation_platform.blend differ
diff --git a/models/meshes/manipulation_platform.dae b/models/meshes/manipulation_platform.dae
index b009d46..13fcb9c 100644
--- a/models/meshes/manipulation_platform.dae
+++ b/models/meshes/manipulation_platform.dae
@@ -5,8 +5,8 @@
Blender User
Blender 2.71.0 commit date:2014-06-12, commit time:18:39, hash:169c95b
- 2015-09-07T18:11:28
- 2015-09-07T18:11:28
+ 2015-09-07T18:24:02
+ 2015-09-07T18:24:02
Z_UP
@@ -182,7 +182,7 @@
- 1 0 0 0 0 1 0 0 0 0 1 0 0 0 0 1
+ 0.8208293 0 0 0 0 1.741765 0 0 0 0 1 0 0 0 0 1
diff --git a/models/meshes/manipulation_platform.stl b/models/meshes/manipulation_platform.stl
index 4e0a2db..7c24920 100644
Binary files a/models/meshes/manipulation_platform.stl and b/models/meshes/manipulation_platform.stl differ
diff --git a/scripts/marker_teleop.py b/scripts/marker_teleop.py
index cae5242..745dd01 100755
--- a/scripts/marker_teleop.py
+++ b/scripts/marker_teleop.py
@@ -131,7 +131,7 @@ def menu_grasp_cb(self, msg):
if not checked:
self.menu_handler.setCheckState(handle, MenuHandler.CHECKED)
rospy.loginfo('grasp')
- self.finger_ref = 0.0
+ self.finger_ref = 1.0
else:
self.menu_handler.setCheckState(handle, MenuHandler.UNCHECKED)
self.finger_ref = 0.25
@@ -147,10 +147,10 @@ def menu_release_cb(self, msg):
if not checked:
self.menu_handler.setCheckState(handle, MenuHandler.CHECKED)
rospy.loginfo('release')
- self.finger_ref = 1.0
+ self.finger_ref = 0.0
else:
self.menu_handler.setCheckState(handle, MenuHandler.UNCHECKED)
- self.finger_ref = 0.25
+ self.finger_ref = 0.50
self.transform.header.stamp = rospy.Time.now()
@@ -250,7 +250,7 @@ def cmd_thread(self):
self.publish_cmd_ring_markers(rospy.Time.now())
# Broadcast the command if it's defined
- self.publish_cmd(self.resync_pose, self.finger_ref, rospy.Time.now())
+ self.publish_cmd(self.resync_pose, False, self.finger_ref, rospy.Time.now())
r.sleep()
diff --git a/src/lcsr_barrett/wam_teleop.py b/src/lcsr_barrett/wam_teleop.py
index 207b70e..c29c520 100644
--- a/src/lcsr_barrett/wam_teleop.py
+++ b/src/lcsr_barrett/wam_teleop.py
@@ -64,6 +64,7 @@ def __init__(self, input_ref_frame_id, input_frame_id):
# Command state
self.cmd_frame = None
self.deadman_engaged = False
+ self.engage_augmenter = False
self.cmd_scaling = 0.0
# Hand structures
@@ -84,12 +85,12 @@ def __init__(self, input_ref_frame_id, input_frame_id):
sensor_msgs.msg.JointState,
self.hand_state_cb)
- self.hand_pub = rospy.Publisher('hand/cmd', oro_barrett_msgs.msg.BHandCmd)
+ self.hand_pub = rospy.Publisher('hand/cmd', oro_barrett_msgs.msg.BHandCmd, queue_size=1000)
# ROS generic telemanip command
- self.telemanip_cmd_pub = rospy.Publisher('telemanip_cmd_out', TelemanipCommand)
+ self.telemanip_cmd_pub = rospy.Publisher('telemanip_cmd_out', TelemanipCommand, queue_size=1000)
# goal marker
- self.marker_pub = rospy.Publisher('master_target_markers', MarkerArray)
+ self.marker_pub = rospy.Publisher('master_target_markers', MarkerArray, queue_size=1000)
self.master_target_markers = MarkerArray()
@@ -214,7 +215,7 @@ def handle_cart_cmd(self, scaling):
# Update commanded TF frame
cmd_twist = kdl.diff(self.cmd_origin, input_frame)
cmd_twist.vel = self.scale*self.cart_scaling*cmd_twist.vel
- rospy.logwarn(cmd_twist)
+ #rospy.logwarn(cmd_twist)
self.cmd_frame = kdl.addDelta(self.tip_origin, cmd_twist)
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException) as ex: