diff --git a/README.md b/README.md
index 3149d2f..337f1ad 100644
--- a/README.md
+++ b/README.md
@@ -1,3 +1,8 @@
+# OAkyildiz/reflex-ros-pkg
+fork of RightHandRobotics/reflex-ros-pkg
+This fork aims to fix the catkin build order problems and namespace issues
+
+
reflex-ros-pkg
======
diff --git a/reflex/launch/reflex_plus.launch b/reflex/launch/reflex_plus.launch
index a57bc06..3f5935b 100644
--- a/reflex/launch/reflex_plus.launch
+++ b/reflex/launch/reflex_plus.launch
@@ -23,5 +23,5 @@
--port reflex_plus_port
reflex_plus_f1 reflex_plus_f2 reflex_plus_f3 reflex_plus_preshape"
output="screen"/>
-
+
diff --git a/reflex/launch/reflex_sf.launch b/reflex/launch/reflex_sf.launch
index c5763b4..0a7c573 100644
--- a/reflex/launch/reflex_sf.launch
+++ b/reflex/launch/reflex_sf.launch
@@ -22,5 +22,5 @@
--port reflex_sf_port
reflex_sf_f1 reflex_sf_f2 reflex_sf_f3 reflex_sf_preshape"
output="screen"/>
-
+
diff --git a/reflex/src/reflex/motor.py b/reflex/src/reflex/motor.py
index 2a0e876..3f48cf2 100644
--- a/reflex/src/reflex/motor.py
+++ b/reflex/src/reflex/motor.py
@@ -33,7 +33,15 @@ def __init__(self, name):
Assumes that "name" is the name of the controller with a preceding
slash, e.g. /reflex_takktile_f1 or /reflex_sf_f1
'''
- self.name = name[1:]
+ # self.name = name[1:] # prone-tofail
+ # this is better if RHR wants to keep it:
+ # self.name=name[1:] if name[0] ='/' else name
+
+ '''
+ But why do that, and then manually add slash? Instead:
+ '''
+ self.name=name
+
self._DEFAULT_MOTOR_SPEED = rospy.get_param(name + '/default_motor_speed')
self._MAX_MOTOR_SPEED = rospy.get_param(name + '/max_motor_speed')
self._MAX_MOTOR_TRAVEL = rospy.get_param(name + '/max_motor_travel')
diff --git a/reflex/src/reflex/reflex_usb_hand.py b/reflex/src/reflex/reflex_usb_hand.py
index 8b2160e..f55ca32 100755
--- a/reflex/src/reflex/reflex_usb_hand.py
+++ b/reflex/src/reflex/reflex_usb_hand.py
@@ -40,10 +40,12 @@
motor_names = ['_f1', '_f2', '_f3', '_preshape']
class ReflexUSBHand(ReflexHand):
- def __init__(self):
- self.usb_hand_type = rospy.get_param('usb_hand_type')
- self.init_namespace = '/' + self.usb_hand_type
+ def __init__(self,name):
+ self.usb_hand_type = rospy.get_param('usb_hand_type')
+ self.init_namespace = name
+
super(ReflexUSBHand, self).__init__(self.init_namespace, ReflexUSBMotor)
+
self.hand_state_pub = rospy.Publisher(self.namespace + '/hand_state',
reflex_msgs.msg.Hand, queue_size=10)
self.encoder_last_value = [0, 0, 0] #This will be updated constantly in _receive_enc_state_cb()
@@ -57,10 +59,10 @@ def __init__(self):
if (self.usb_hand_type == "reflex_plus"):
self.enc_subscriber = rospy.Subscriber('/encoder_states', Encoder, self._receive_enc_state_cb)
rospy.Service(self.namespace + '/calibrate_fingers', Empty, self.calibrate_auto)
- self.encoder_zero_point = rospy.get_param('/enc_zero_points')
+ self.encoder_zero_point = rospy.get_param('/enc_zero_points') #remove slash?
else:
rospy.Service(self.namespace + '/calibrate_fingers', Empty, self.calibrate_manual)
-
+
def _receive_enc_state_cb(self, data):
#Receives and processes the encoder state
#print("encoder 1: " + str(data.encoders[0]) + " encoder 2: " + str(data.encoders[1]) + " encoder 3: " + str(data.encoders[2]))
@@ -74,7 +76,7 @@ def _receive_enc_state_cb(self, data):
#motor_angles[i] = raw_motor_angle
self.distal_approx[i] = self.calc_distal_angle(motor_joint_angle, self.proximal_angle[i])
#print motor_angles
-
+
def _receive_cmd_cb(self, data):
self.disable_force_control()
self.set_speeds(data.velocity)
@@ -143,7 +145,7 @@ def calibrate_auto(self, data=None):
j=0
while(1):
enc_pos = self.encoder_last_value[i]
- if (j==0):
+ if (j==0):
j=1
last = enc_pos
if ((abs(enc_pos-last) < self.calibration_error)):
@@ -172,20 +174,20 @@ def _write_zero_point_data_to_file(self, filename, data):
outfile.write(yaml.dump(data))
def _zero_current_pose(self):
- data = dict(
- reflex_sf_f1=dict(zero_point=self.motors[self.namespace + '_f1'].get_current_raw_motor_angle()),
- reflex_sf_f2=dict(zero_point=self.motors[self.namespace + '_f2'].get_current_raw_motor_angle()),
- reflex_sf_f3=dict(zero_point=self.motors[self.namespace + '_f3'].get_current_raw_motor_angle()),
- reflex_sf_preshape=dict(zero_point=self.motors[self.namespace + '_preshape'].get_current_raw_motor_angle())
- )
+ data = dict()
+ data[self.namespace+"_f1"]=dict(zero_point=self.motors[self.namespace + "_f1"].get_current_raw_motor_angle())
+ data[self.namespace+"_f2"]=dict(zero_point=self.motors[self.namespace + "_f2"].get_current_raw_motor_angle())
+ data[self.namespace+"_f3"]=dict(zero_point=self.motors[self.namespace + "_f3"].get_current_raw_motor_angle())
+ data[self.namespace+"_preshape"]=dict(zero_point=self.motors[self.namespace + "_preshape"].get_current_raw_motor_angle())
+
if (self.usb_hand_type == 'reflex_plus'):
data = dict(
- reflex_plus_f1 = data['reflex_sf_f1'],
- reflex_plus_f2 = data['reflex_sf_f2'],
- reflex_plus_f3 = data['reflex_sf_f3'],
- reflex_plus_preshape = data['reflex_sf_preshape']
+ reflex_plus_f1 = data[self.namespace+'_f1'],
+ reflex_plus_f2 = data[self.namespace+'_f2'],
+ reflex_plus_f3 = data[self.namespace+'_f3'],
+ reflex_plus_preshape = data[self.namespace+'_preshape']
)
- self._write_zero_point_data_to_file(self.usb_hand_type + '_motor_zero_points.yaml', data)
+ self._write_zero_point_data_to_file(self.namespace + '_motor_zero_points.yaml', data)
#Encoder data processing functions are based off encoder functions used
#For the reflex_takktile hand as in reflex_driver_node.cpp
@@ -195,12 +197,12 @@ def calibrate_encoders_locally(self, data):
self.encoder_zero_point[i] = data[i]*self.enc_scale
self.encoder_offset[i] = 0;
data = dict(enc_zero_points = self.encoder_zero_point)
- self._write_zero_point_data_to_file('reflex_plus_encoder_zero_points.yaml', data)
+ self._write_zero_point_data_to_file('reflex_plus_encoder_zero_points.yaml', data)
def update_encoder_offset(self, raw_value):
#Given a raw and past (self.encoder_last_value) value, track encoder wrapes (self.enc_offset)
offset = self.encoder_offset[:]
-
+
for i in range(0,3):
if (offset[i]==-1):
#This happens at start up
@@ -211,7 +213,7 @@ def update_encoder_offset(self, raw_value):
offset[i] = offset[i] + 16383
elif (self.encoder_last_value[i] - raw_value[i] < -5000):
offset[i] = offset[i] - 16383
-
+
self.encoder_offset = offset[:]
def calc_proximal_angle(self, raw_value, zero, offset):
@@ -230,9 +232,9 @@ def calc_distal_angle(self, joint_angle, proximal):
else:
return diff
-def main():
+def main(name):
rospy.sleep(4.0) # To allow services and parameters to load
- hand = ReflexUSBHand()
+ hand = ReflexUSBHand(name)
rospy.on_shutdown(hand.disable_torque)
r = rospy.Rate(20)
while not rospy.is_shutdown():
@@ -241,4 +243,7 @@ def main():
if __name__ == '__main__':
- main()
+ import sys
+ #this needs to be slightly revised as and empty 'args' parameter in roslaunch causes problems
+ nm='reflex_sf' if len(sys.argv)<=1 else sys.argv[1]
+ main(nm)
diff --git a/reflex/src/reflex/reflex_usb_motor.py b/reflex/src/reflex/reflex_usb_motor.py
index 7da4cd4..ce09ed0 100644
--- a/reflex/src/reflex/reflex_usb_motor.py
+++ b/reflex/src/reflex/reflex_usb_motor.py
@@ -33,7 +33,7 @@
class ReflexUSBMotor(Motor):
def __init__(self, name):
super(ReflexUSBMotor, self).__init__(name)
- self.zero_point = rospy.get_param(self.name + '/zero_point')
+ self.zero_point = rospy.get_param(self.name + "/zero_point")
self.MOTOR_TO_JOINT_GEAR_RATIO = rospy.get_param(self.name + '/motor_to_joint_gear_ratio')
self.MOTOR_TO_JOINT_INVERTED = rospy.get_param(self.name + '/motor_to_joint_inverted')
self.motor_cmd_pub = rospy.Publisher(name + '/command', Float64, queue_size=10)
diff --git a/reflex/yaml/left_hand_motor_zero_points.yaml b/reflex/yaml/left_hand_motor_zero_points.yaml
new file mode 100644
index 0000000..6b2dca0
--- /dev/null
+++ b/reflex/yaml/left_hand_motor_zero_points.yaml
@@ -0,0 +1,4 @@
+reflex_sf_f1: {zero_point: 20.95724552409363}
+reflex_sf_f2: {zero_point: 20.28075999663606}
+reflex_sf_f3: {zero_point: 24.543692606170257}
+reflex_sf_preshape: {zero_point: 21.764119418521478}
diff --git a/reflex/yaml/right_hand_motor_zero_points.yaml b/reflex/yaml/right_hand_motor_zero_points.yaml
new file mode 100644
index 0000000..67f6d1b
--- /dev/null
+++ b/reflex/yaml/right_hand_motor_zero_points.yaml
@@ -0,0 +1,4 @@
+reflex_sf_f1: {zero_point: 22.929944817314563}
+reflex_sf_f2: {zero_point: 20.469439633545996}
+reflex_sf_f3: {zero_point: 22.88545937446588}
+reflex_sf_preshape: {zero_point: 23.58955655610539}
diff --git a/reflex/yaml/this_is_left_hand.yaml b/reflex/yaml/this_is_left_hand.yaml
new file mode 100644
index 0000000..2dae907
--- /dev/null
+++ b/reflex/yaml/this_is_left_hand.yaml
@@ -0,0 +1,4 @@
+reflex_sf_f1: {zero_point: 20.773167829547354}
+reflex_sf_f2: {zero_point: 20.49244934536428}
+reflex_sf_f3: {zero_point: 25.615945176902322}
+reflex_sf_preshape: {zero_point: 21.567769877672117}
diff --git a/reflex_visualizer/launch/reflex_visualizer.launch b/reflex_visualizer/launch/reflex_visualizer.launch
index 48edb56..59accc9 100644
--- a/reflex_visualizer/launch/reflex_visualizer.launch
+++ b/reflex_visualizer/launch/reflex_visualizer.launch
@@ -1,3 +1,4 @@
+
diff --git a/reflex_visualizer/urdf/full_reflex_model.urdf.xacro b/reflex_visualizer/urdf/full_reflex_model.urdf.xacro
index a1fb1c5..add3928 100644
--- a/reflex_visualizer/urdf/full_reflex_model.urdf.xacro
+++ b/reflex_visualizer/urdf/full_reflex_model.urdf.xacro
@@ -1,7 +1,10 @@
-
+
+
+
+
@@ -34,12 +37,12 @@
-
+
+ name="${tf_prefix}finger[${finger_id}]/flex_link[${flex_id}]">
+ name="${tf_prefix}">
@@ -58,9 +61,9 @@
-
-
-
+
+
+
@@ -68,81 +71,89 @@
-
-
-
+
+
+
-
-
-
+
+
+
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
@@ -150,534 +161,525 @@
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+