From f1b6acb3357c005e4a9ff1b90700821588d8e739 Mon Sep 17 00:00:00 2001 From: Brent Yi Date: Mon, 5 Nov 2018 23:16:37 -0600 Subject: [PATCH 01/12] accelerometer service framework --- blue_bringup/scripts/accel_calibration.py | 97 +++++++++++++++++++++++ 1 file changed, 97 insertions(+) create mode 100755 blue_bringup/scripts/accel_calibration.py diff --git a/blue_bringup/scripts/accel_calibration.py b/blue_bringup/scripts/accel_calibration.py new file mode 100755 index 00000000..c91ca41b --- /dev/null +++ b/blue_bringup/scripts/accel_calibration.py @@ -0,0 +1,97 @@ +#!/usr/bin/env python + +"""This node should run at startup, and sets the initial joint angles to some hardcoded values.""" + +import rospy +import sys +import actionlib +from std_msgs.msg import Int32 +from std_msgs.msg import Float32 +from geometry_msgs.msg import PoseStamped +from sensor_msgs.msg import JointState +import PyKDL as kdl +import kdl_parser_py.urdf as kdl_parser +from blue_msgs.srv import JointStartupCalibration + +class ParticleFilter: + def __init__(self, num_joints, differential_pairs, ): + + +class particle_group: + def __init__(self, num_joints, num_particles, joint_min, joint_max): + self.num_j = num_joints + self.num_p = num_particles + self.jmin = joint_min + self.jmax = joint_max + self.particle_offsets = np.uniform.random(self.jmin, self.jmax, (num_p, num_j)) + +class BlueRobotFilter: + def _setup(self): + # load in ros parameters + self.baselink = rospy.get_param("blue_hardware/baselink") + self.endlink = rospy.get_param("blue_harware/endlink") + flag, self.tree = kdl_parser.treeFromParam("/robot_description") + + self.joint_names = rospy.get_param("blue_hardware/joint_names") + print self.joint_names + + # build kinematic chain and fk and jacobian solvers + chain_ee = self.tree.getChain(self.baselink, self.endlink) + self.fk_ee = kdl.ChainFkSolverPos_recursive(chain_ee) + self.jac_ee = kdl.ChainJntToJacSolver(chain_ee) + + # building robot joint state + self.num_joints = chain_ee.getNrOfJoints() + self.joints = kdl.JntArray(self.num_joints) + + def __init__(self, debug=False): + self.debug = debug + if self.debug: + self.debug_count = 0 + self._setup() + + def update_joints(self, joint_msg): + new_joints = self.joints.copy() + for i, n in enumerate(self.joint_names): + index = joint_msg.name.index(n) + self.joints[i] = joint_msg.position[index] + + def calibrate(self): + rospy.Subscriber("/joint_states", JointState, self.update_joints) + +if __name__ == "__main__": + rospy.init_node("simple_startup_calibration") + + # initalize filter + + + # building the kinematic chains + rospy.loginfo("Building Blue Object...") + blue = BlueRobotFilter(debug=True) + blue.calibrate() + r = rospy.Rate(1.0) + while blue.probability() < 0.9: + r.sleep(); + + # Read startup angles from parameter server + startup_positions = blue.get_best_estimate(); + + # Wait for calibration service to come up + rospy.loginfo("Waiting for calibration service...") + rospy.wait_for_service('blue_hardware/joint_startup_calibration') + + # Calibrate joints with startup angles + rospy.loginfo("Starting calibration...") + try: + joint_startup_calibration = rospy.ServiceProxy('blue_hardware/joint_startup_calibration', JointStartupCalibration) + response = joint_startup_calibration(startup_positions) + + if response.success: + rospy.loginfo("Joint startup calibration succeeded!") + else: + rospy.logerr("Joint startup calibration failed!") + + except rospy.ServiceException as e: + rospy.logerr("Joint startup calibration failed: %s" % e) + + From d16348e0f82929ac7fc27729776ee250c790ca00 Mon Sep 17 00:00:00 2001 From: Brent Yi Date: Tue, 6 Nov 2018 01:56:20 -0600 Subject: [PATCH 02/12] calibration --- blue_bringup/scripts/accel_calibration.py | 100 ++++++++++++++++++---- 1 file changed, 81 insertions(+), 19 deletions(-) diff --git a/blue_bringup/scripts/accel_calibration.py b/blue_bringup/scripts/accel_calibration.py index c91ca41b..0cc83787 100755 --- a/blue_bringup/scripts/accel_calibration.py +++ b/blue_bringup/scripts/accel_calibration.py @@ -13,36 +13,97 @@ import kdl_parser_py.urdf as kdl_parser from blue_msgs.srv import JointStartupCalibration -class ParticleFilter: - def __init__(self, num_joints, differential_pairs, ): - - -class particle_group: - def __init__(self, num_joints, num_particles, joint_min, joint_max): +# http://docs.ros.org/indigo/api/message_filters/html/python/#message_filters.TimeSynchronizer +# synch callbacks with this + +# class ParticleFilter: + # def __init__(self, num_joints, differential_pairs, particles_per_joint, jmin, jmax): +# + # def update(self, new_joints, accel): + # TODO, update ParticleGroups + # return mle_joints, mle +def measurement_likelihood(vec_part, vec_meas): + return np.linalg.norm(vec_part.dot(vec_meas)) + +class ParticleGroup: + def __init__(self, num_joints, num_particles, joint_min, joint_max, fk): self.num_j = num_joints self.num_p = num_particles self.jmin = joint_min self.jmax = joint_max + self.fk_solver = fk self.particle_offsets = np.uniform.random(self.jmin, self.jmax, (num_p, num_j)) + def update(self, joints, accel_1, accel_2): + # TODO figure out accel message type + ml_array = [] + for p in self.particle_offsets: + frame = kdl.Frame() + self.fk_solver.JntToCart(joints + p, frame) + grav_prev = kdl.Vector() + grav_prev.data = accel_1 + grav_post = frame * grav_prev + ml = measurement_likelihood(grav_post, accel_2) + ml_array.append(ml) + + # TODO reample particles and update particle filter offsets + self.particle_offsets = np.random.uniform(self.jmin, self.jmax, (num_p, num_j)) + + # TODO implement roughening + # some code that adds noise + + # TODO return max joint angles, and max likelyhood probability + return ml_joints, ml + + class BlueRobotFilter: def _setup(self): # load in ros parameters self.baselink = rospy.get_param("blue_hardware/baselink") self.endlink = rospy.get_param("blue_harware/endlink") flag, self.tree = kdl_parser.treeFromParam("/robot_description") - - self.joint_names = rospy.get_param("blue_hardware/joint_names") - print self.joint_names - # build kinematic chain and fk and jacobian solvers chain_ee = self.tree.getChain(self.baselink, self.endlink) - self.fk_ee = kdl.ChainFkSolverPos_recursive(chain_ee) - self.jac_ee = kdl.ChainJntToJacSolver(chain_ee) # building robot joint state self.num_joints = chain_ee.getNrOfJoints() - self.joints = kdl.JntArray(self.num_joints) + + # other joint information + self.joint_names = rospy.get_param("blue_hardware/joint_names") + self.accel_links = rospy.get_param("blue_hardware/accel_links").insert(0, self.baselink) + self.accel_links.insert(0, self.baselink) + print self.joint_names + + particles_per_joints = 100 + +#### + ppj = particles_per_joint + p_groups = [] + joints_per_group = [] + i = 0 + g = 0 + while i < num_joints: + if i in differential_pairs: + chain = self.tree.getChain(self.accel_links[g], self.accel_links[g+1]) + fk = kdl.ChainFkSolverPos_recursive(chain) + pg = ParticleGroup(2, ppj, [jmin[i], jmin[i+1]], [jmax[i], jmax[i+1]], fk) + + p_groups.append(pg) + joints_per_group.append(2) + i = i + 1 + else: + chain = self.tree.getChain(self.accel_links[g], self.accel_links[g+1]) + fk = kdl.ChainFkSolverPos_recursive(chain) + pg = ParticleGroup(1, ppj, [jmin[i]], [jmax[i]], fk) + + p_groups.append(pg) + joints_per_group.append(1) + i = i + 1 + g = g + 1 +#### + + self.pg_array = p_groups + self.jpg = joints_per_group def __init__(self, debug=False): self.debug = debug @@ -51,8 +112,14 @@ def __init__(self, debug=False): self._setup() def update_joints(self, joint_msg): + # TODO figure out how to synchronize with accelerometer + pass + + def update_filter(self, joints, accel): new_joints = self.joints.copy() - for i, n in enumerate(self.joint_names): + i = 0 + for j, pg in enumerate(self.pg_array): + index = joint_msg.name.index(n) self.joints[i] = joint_msg.position[index] @@ -62,9 +129,6 @@ def calibrate(self): if __name__ == "__main__": rospy.init_node("simple_startup_calibration") - # initalize filter - - # building the kinematic chains rospy.loginfo("Building Blue Object...") blue = BlueRobotFilter(debug=True) @@ -93,5 +157,3 @@ def calibrate(self): except rospy.ServiceException as e: rospy.logerr("Joint startup calibration failed: %s" % e) - - From de45224821595287eea9a2deba500ac8eef6542e Mon Sep 17 00:00:00 2001 From: Brent Yi Date: Tue, 6 Nov 2018 19:21:29 -0600 Subject: [PATCH 03/12] over all structure finished --- blue_bringup/scripts/accel_calibration.py | 55 ++++++++++++++--------- 1 file changed, 35 insertions(+), 20 deletions(-) diff --git a/blue_bringup/scripts/accel_calibration.py b/blue_bringup/scripts/accel_calibration.py index 0cc83787..b9b691ce 100755 --- a/blue_bringup/scripts/accel_calibration.py +++ b/blue_bringup/scripts/accel_calibration.py @@ -16,12 +16,6 @@ # http://docs.ros.org/indigo/api/message_filters/html/python/#message_filters.TimeSynchronizer # synch callbacks with this -# class ParticleFilter: - # def __init__(self, num_joints, differential_pairs, particles_per_joint, jmin, jmax): -# - # def update(self, new_joints, accel): - # TODO, update ParticleGroups - # return mle_joints, mle def measurement_likelihood(vec_part, vec_meas): return np.linalg.norm(vec_part.dot(vec_meas)) @@ -46,15 +40,19 @@ def update(self, joints, accel_1, accel_2): ml = measurement_likelihood(grav_post, accel_2) ml_array.append(ml) - # TODO reample particles and update particle filter offsets - self.particle_offsets = np.random.uniform(self.jmin, self.jmax, (num_p, num_j)) - # TODO implement roughening - # some code that adds noise + alpha = np.sum(ml_array) + ml_array = np.array(ml_array) / alpha + cdf = np.cumsum(ml_array) + # TODO verify that this is done correctly + self.particle_offsets = np.array([self.particle_offsets[np.argwhere(cdf>np.random.uniform())[0,0],:] for i in range(self.num_p)]) - # TODO return max joint angles, and max likelyhood probability - return ml_joints, ml + # TODO implement roughening + if False: + pass + index = np.argmax(ml_array) + return self.particle_offsets[index,:], ml_array[index] class BlueRobotFilter: def _setup(self): @@ -72,11 +70,12 @@ def _setup(self): self.joint_names = rospy.get_param("blue_hardware/joint_names") self.accel_links = rospy.get_param("blue_hardware/accel_links").insert(0, self.baselink) self.accel_links.insert(0, self.baselink) - print self.joint_names + self.best_estimate = np.zeros(self.num_joints) + self.probability = 0 + # TODO make a rosparam particles_per_joints = 100 -#### ppj = particles_per_joint p_groups = [] joints_per_group = [] @@ -100,7 +99,6 @@ def _setup(self): joints_per_group.append(1) i = i + 1 g = g + 1 -#### self.pg_array = p_groups self.jpg = joints_per_group @@ -111,20 +109,37 @@ def __init__(self, debug=False): self.debug_count = 0 self._setup() - def update_joints(self, joint_msg): - # TODO figure out how to synchronize with accelerometer - pass + def get_probability(self): + return self.probability + + def get_best_estimate(self): + return self.best_estimate def update_filter(self, joints, accel): new_joints = self.joints.copy() i = 0 + probabilities = [] + best_estimate = [] for j, pg in enumerate(self.pg_array): - index = joint_msg.name.index(n) self.joints[i] = joint_msg.position[index] + new_link_joints = [] + for k in range(self.jpg[j]): + new_link_joints.append(joint_msg.position[i]) + i = i + 1 + joints, prob = pg.update(new_link_joints, self.accel_links[j], self.accel_links[j+1]) + probabilities.append(prob) + best_estimate.append(joints) + + self.best_estimate = np.array(best_estimate) + self.probability = np.min(probabilities) def calibrate(self): + # TODO write this function which uses the synchronized callbacks + # rospy.Subscriber("/accel", Brents custom accel, self.update_joints) rospy.Subscriber("/joint_states", JointState, self.update_joints) + # synch callbacks with this + # sych callback = update filter if __name__ == "__main__": rospy.init_node("simple_startup_calibration") @@ -134,7 +149,7 @@ def calibrate(self): blue = BlueRobotFilter(debug=True) blue.calibrate() r = rospy.Rate(1.0) - while blue.probability() < 0.9: + while blue.get_probability() < 0.9: r.sleep(); # Read startup angles from parameter server From e82cd88a93ba4ffe9fefd89393fb565af900f635 Mon Sep 17 00:00:00 2001 From: Brent Yi Date: Wed, 7 Nov 2018 00:09:35 -0600 Subject: [PATCH 04/12] removed calibration angles from right arm configuration --- blue_bringup/config/robot_parameters_right.yaml | 3 --- 1 file changed, 3 deletions(-) diff --git a/blue_bringup/config/robot_parameters_right.yaml b/blue_bringup/config/robot_parameters_right.yaml index 929ff24d..db77c003 100644 --- a/blue_bringup/config/robot_parameters_right.yaml +++ b/blue_bringup/config/robot_parameters_right.yaml @@ -39,9 +39,6 @@ blue_hardware: motor_current_limits: [2.0, 2.0, 2.0, 2.0, 2.0, 1.0, 1.0, 1.0] current_to_torque_ratios: [0.626, 0.626, 0.626, 0.626, 0.626, 1.67, 1.67, 1.67] - ### Calibration configuration - simple_startup_angles: [0.785398, -2.19, 1.570796, 0.0, -1.570796, -0.23, 0.0, 0.0] - ### Posture control parameters for gradient descent ik pose controller posture_target: [-0.9, -1.57, 0.0, -1.57, -0.90, -1.0, 0.0] posture_weights: [0.2, 1, 1, 1, 1, 0.2, 0.5] From e065be84003eae6d35ab657ebb8a946134d46aa1 Mon Sep 17 00:00:00 2001 From: Brent Yi Date: Mon, 5 Nov 2018 23:16:37 -0600 Subject: [PATCH 05/12] accelerometer service framework --- blue_bringup/scripts/accel_calibration.py | 97 +++++++++++++++++++++++ 1 file changed, 97 insertions(+) create mode 100755 blue_bringup/scripts/accel_calibration.py diff --git a/blue_bringup/scripts/accel_calibration.py b/blue_bringup/scripts/accel_calibration.py new file mode 100755 index 00000000..c91ca41b --- /dev/null +++ b/blue_bringup/scripts/accel_calibration.py @@ -0,0 +1,97 @@ +#!/usr/bin/env python + +"""This node should run at startup, and sets the initial joint angles to some hardcoded values.""" + +import rospy +import sys +import actionlib +from std_msgs.msg import Int32 +from std_msgs.msg import Float32 +from geometry_msgs.msg import PoseStamped +from sensor_msgs.msg import JointState +import PyKDL as kdl +import kdl_parser_py.urdf as kdl_parser +from blue_msgs.srv import JointStartupCalibration + +class ParticleFilter: + def __init__(self, num_joints, differential_pairs, ): + + +class particle_group: + def __init__(self, num_joints, num_particles, joint_min, joint_max): + self.num_j = num_joints + self.num_p = num_particles + self.jmin = joint_min + self.jmax = joint_max + self.particle_offsets = np.uniform.random(self.jmin, self.jmax, (num_p, num_j)) + +class BlueRobotFilter: + def _setup(self): + # load in ros parameters + self.baselink = rospy.get_param("blue_hardware/baselink") + self.endlink = rospy.get_param("blue_harware/endlink") + flag, self.tree = kdl_parser.treeFromParam("/robot_description") + + self.joint_names = rospy.get_param("blue_hardware/joint_names") + print self.joint_names + + # build kinematic chain and fk and jacobian solvers + chain_ee = self.tree.getChain(self.baselink, self.endlink) + self.fk_ee = kdl.ChainFkSolverPos_recursive(chain_ee) + self.jac_ee = kdl.ChainJntToJacSolver(chain_ee) + + # building robot joint state + self.num_joints = chain_ee.getNrOfJoints() + self.joints = kdl.JntArray(self.num_joints) + + def __init__(self, debug=False): + self.debug = debug + if self.debug: + self.debug_count = 0 + self._setup() + + def update_joints(self, joint_msg): + new_joints = self.joints.copy() + for i, n in enumerate(self.joint_names): + index = joint_msg.name.index(n) + self.joints[i] = joint_msg.position[index] + + def calibrate(self): + rospy.Subscriber("/joint_states", JointState, self.update_joints) + +if __name__ == "__main__": + rospy.init_node("simple_startup_calibration") + + # initalize filter + + + # building the kinematic chains + rospy.loginfo("Building Blue Object...") + blue = BlueRobotFilter(debug=True) + blue.calibrate() + r = rospy.Rate(1.0) + while blue.probability() < 0.9: + r.sleep(); + + # Read startup angles from parameter server + startup_positions = blue.get_best_estimate(); + + # Wait for calibration service to come up + rospy.loginfo("Waiting for calibration service...") + rospy.wait_for_service('blue_hardware/joint_startup_calibration') + + # Calibrate joints with startup angles + rospy.loginfo("Starting calibration...") + try: + joint_startup_calibration = rospy.ServiceProxy('blue_hardware/joint_startup_calibration', JointStartupCalibration) + response = joint_startup_calibration(startup_positions) + + if response.success: + rospy.loginfo("Joint startup calibration succeeded!") + else: + rospy.logerr("Joint startup calibration failed!") + + except rospy.ServiceException as e: + rospy.logerr("Joint startup calibration failed: %s" % e) + + From 8618dd9c5dc898af0515740800a35fa9a43d387e Mon Sep 17 00:00:00 2001 From: Brent Yi Date: Tue, 6 Nov 2018 01:56:20 -0600 Subject: [PATCH 06/12] calibration --- blue_bringup/scripts/accel_calibration.py | 100 ++++++++++++++++++---- 1 file changed, 81 insertions(+), 19 deletions(-) diff --git a/blue_bringup/scripts/accel_calibration.py b/blue_bringup/scripts/accel_calibration.py index c91ca41b..0cc83787 100755 --- a/blue_bringup/scripts/accel_calibration.py +++ b/blue_bringup/scripts/accel_calibration.py @@ -13,36 +13,97 @@ import kdl_parser_py.urdf as kdl_parser from blue_msgs.srv import JointStartupCalibration -class ParticleFilter: - def __init__(self, num_joints, differential_pairs, ): - - -class particle_group: - def __init__(self, num_joints, num_particles, joint_min, joint_max): +# http://docs.ros.org/indigo/api/message_filters/html/python/#message_filters.TimeSynchronizer +# synch callbacks with this + +# class ParticleFilter: + # def __init__(self, num_joints, differential_pairs, particles_per_joint, jmin, jmax): +# + # def update(self, new_joints, accel): + # TODO, update ParticleGroups + # return mle_joints, mle +def measurement_likelihood(vec_part, vec_meas): + return np.linalg.norm(vec_part.dot(vec_meas)) + +class ParticleGroup: + def __init__(self, num_joints, num_particles, joint_min, joint_max, fk): self.num_j = num_joints self.num_p = num_particles self.jmin = joint_min self.jmax = joint_max + self.fk_solver = fk self.particle_offsets = np.uniform.random(self.jmin, self.jmax, (num_p, num_j)) + def update(self, joints, accel_1, accel_2): + # TODO figure out accel message type + ml_array = [] + for p in self.particle_offsets: + frame = kdl.Frame() + self.fk_solver.JntToCart(joints + p, frame) + grav_prev = kdl.Vector() + grav_prev.data = accel_1 + grav_post = frame * grav_prev + ml = measurement_likelihood(grav_post, accel_2) + ml_array.append(ml) + + # TODO reample particles and update particle filter offsets + self.particle_offsets = np.random.uniform(self.jmin, self.jmax, (num_p, num_j)) + + # TODO implement roughening + # some code that adds noise + + # TODO return max joint angles, and max likelyhood probability + return ml_joints, ml + + class BlueRobotFilter: def _setup(self): # load in ros parameters self.baselink = rospy.get_param("blue_hardware/baselink") self.endlink = rospy.get_param("blue_harware/endlink") flag, self.tree = kdl_parser.treeFromParam("/robot_description") - - self.joint_names = rospy.get_param("blue_hardware/joint_names") - print self.joint_names - # build kinematic chain and fk and jacobian solvers chain_ee = self.tree.getChain(self.baselink, self.endlink) - self.fk_ee = kdl.ChainFkSolverPos_recursive(chain_ee) - self.jac_ee = kdl.ChainJntToJacSolver(chain_ee) # building robot joint state self.num_joints = chain_ee.getNrOfJoints() - self.joints = kdl.JntArray(self.num_joints) + + # other joint information + self.joint_names = rospy.get_param("blue_hardware/joint_names") + self.accel_links = rospy.get_param("blue_hardware/accel_links").insert(0, self.baselink) + self.accel_links.insert(0, self.baselink) + print self.joint_names + + particles_per_joints = 100 + +#### + ppj = particles_per_joint + p_groups = [] + joints_per_group = [] + i = 0 + g = 0 + while i < num_joints: + if i in differential_pairs: + chain = self.tree.getChain(self.accel_links[g], self.accel_links[g+1]) + fk = kdl.ChainFkSolverPos_recursive(chain) + pg = ParticleGroup(2, ppj, [jmin[i], jmin[i+1]], [jmax[i], jmax[i+1]], fk) + + p_groups.append(pg) + joints_per_group.append(2) + i = i + 1 + else: + chain = self.tree.getChain(self.accel_links[g], self.accel_links[g+1]) + fk = kdl.ChainFkSolverPos_recursive(chain) + pg = ParticleGroup(1, ppj, [jmin[i]], [jmax[i]], fk) + + p_groups.append(pg) + joints_per_group.append(1) + i = i + 1 + g = g + 1 +#### + + self.pg_array = p_groups + self.jpg = joints_per_group def __init__(self, debug=False): self.debug = debug @@ -51,8 +112,14 @@ def __init__(self, debug=False): self._setup() def update_joints(self, joint_msg): + # TODO figure out how to synchronize with accelerometer + pass + + def update_filter(self, joints, accel): new_joints = self.joints.copy() - for i, n in enumerate(self.joint_names): + i = 0 + for j, pg in enumerate(self.pg_array): + index = joint_msg.name.index(n) self.joints[i] = joint_msg.position[index] @@ -62,9 +129,6 @@ def calibrate(self): if __name__ == "__main__": rospy.init_node("simple_startup_calibration") - # initalize filter - - # building the kinematic chains rospy.loginfo("Building Blue Object...") blue = BlueRobotFilter(debug=True) @@ -93,5 +157,3 @@ def calibrate(self): except rospy.ServiceException as e: rospy.logerr("Joint startup calibration failed: %s" % e) - - From aa7675d688e4e36c0a6bfb7a61672f21b6c31b78 Mon Sep 17 00:00:00 2001 From: Brent Yi Date: Tue, 6 Nov 2018 19:21:29 -0600 Subject: [PATCH 07/12] over all structure finished --- blue_bringup/scripts/accel_calibration.py | 55 ++++++++++++++--------- 1 file changed, 35 insertions(+), 20 deletions(-) diff --git a/blue_bringup/scripts/accel_calibration.py b/blue_bringup/scripts/accel_calibration.py index 0cc83787..b9b691ce 100755 --- a/blue_bringup/scripts/accel_calibration.py +++ b/blue_bringup/scripts/accel_calibration.py @@ -16,12 +16,6 @@ # http://docs.ros.org/indigo/api/message_filters/html/python/#message_filters.TimeSynchronizer # synch callbacks with this -# class ParticleFilter: - # def __init__(self, num_joints, differential_pairs, particles_per_joint, jmin, jmax): -# - # def update(self, new_joints, accel): - # TODO, update ParticleGroups - # return mle_joints, mle def measurement_likelihood(vec_part, vec_meas): return np.linalg.norm(vec_part.dot(vec_meas)) @@ -46,15 +40,19 @@ def update(self, joints, accel_1, accel_2): ml = measurement_likelihood(grav_post, accel_2) ml_array.append(ml) - # TODO reample particles and update particle filter offsets - self.particle_offsets = np.random.uniform(self.jmin, self.jmax, (num_p, num_j)) - # TODO implement roughening - # some code that adds noise + alpha = np.sum(ml_array) + ml_array = np.array(ml_array) / alpha + cdf = np.cumsum(ml_array) + # TODO verify that this is done correctly + self.particle_offsets = np.array([self.particle_offsets[np.argwhere(cdf>np.random.uniform())[0,0],:] for i in range(self.num_p)]) - # TODO return max joint angles, and max likelyhood probability - return ml_joints, ml + # TODO implement roughening + if False: + pass + index = np.argmax(ml_array) + return self.particle_offsets[index,:], ml_array[index] class BlueRobotFilter: def _setup(self): @@ -72,11 +70,12 @@ def _setup(self): self.joint_names = rospy.get_param("blue_hardware/joint_names") self.accel_links = rospy.get_param("blue_hardware/accel_links").insert(0, self.baselink) self.accel_links.insert(0, self.baselink) - print self.joint_names + self.best_estimate = np.zeros(self.num_joints) + self.probability = 0 + # TODO make a rosparam particles_per_joints = 100 -#### ppj = particles_per_joint p_groups = [] joints_per_group = [] @@ -100,7 +99,6 @@ def _setup(self): joints_per_group.append(1) i = i + 1 g = g + 1 -#### self.pg_array = p_groups self.jpg = joints_per_group @@ -111,20 +109,37 @@ def __init__(self, debug=False): self.debug_count = 0 self._setup() - def update_joints(self, joint_msg): - # TODO figure out how to synchronize with accelerometer - pass + def get_probability(self): + return self.probability + + def get_best_estimate(self): + return self.best_estimate def update_filter(self, joints, accel): new_joints = self.joints.copy() i = 0 + probabilities = [] + best_estimate = [] for j, pg in enumerate(self.pg_array): - index = joint_msg.name.index(n) self.joints[i] = joint_msg.position[index] + new_link_joints = [] + for k in range(self.jpg[j]): + new_link_joints.append(joint_msg.position[i]) + i = i + 1 + joints, prob = pg.update(new_link_joints, self.accel_links[j], self.accel_links[j+1]) + probabilities.append(prob) + best_estimate.append(joints) + + self.best_estimate = np.array(best_estimate) + self.probability = np.min(probabilities) def calibrate(self): + # TODO write this function which uses the synchronized callbacks + # rospy.Subscriber("/accel", Brents custom accel, self.update_joints) rospy.Subscriber("/joint_states", JointState, self.update_joints) + # synch callbacks with this + # sych callback = update filter if __name__ == "__main__": rospy.init_node("simple_startup_calibration") @@ -134,7 +149,7 @@ def calibrate(self): blue = BlueRobotFilter(debug=True) blue.calibrate() r = rospy.Rate(1.0) - while blue.probability() < 0.9: + while blue.get_probability() < 0.9: r.sleep(); # Read startup angles from parameter server From 6323d04ee99753ddcfd4e9c01b62f43cb9b0ee1f Mon Sep 17 00:00:00 2001 From: Philipp Date: Sat, 24 Nov 2018 14:25:40 -0800 Subject: [PATCH 08/12] everything written need to debug --- blue_bringup/launch/accel_test.launch | 9 ++ .../include/single_arm_control.launch.xml | 16 ++- blue_bringup/scripts/accel_calibration.py | 118 +++++++++++++----- 3 files changed, 106 insertions(+), 37 deletions(-) create mode 100644 blue_bringup/launch/accel_test.launch diff --git a/blue_bringup/launch/accel_test.launch b/blue_bringup/launch/accel_test.launch new file mode 100644 index 00000000..eaad8208 --- /dev/null +++ b/blue_bringup/launch/accel_test.launch @@ -0,0 +1,9 @@ + + + + + diff --git a/blue_bringup/launch/include/single_arm_control.launch.xml b/blue_bringup/launch/include/single_arm_control.launch.xml index a2961f6e..2e7eebda 100644 --- a/blue_bringup/launch/include/single_arm_control.launch.xml +++ b/blue_bringup/launch/include/single_arm_control.launch.xml @@ -38,11 +38,17 @@ - + + + + + + + + + + + diff --git a/blue_bringup/scripts/accel_calibration.py b/blue_bringup/scripts/accel_calibration.py index b9b691ce..f08a500f 100755 --- a/blue_bringup/scripts/accel_calibration.py +++ b/blue_bringup/scripts/accel_calibration.py @@ -12,12 +12,18 @@ import PyKDL as kdl import kdl_parser_py.urdf as kdl_parser from blue_msgs.srv import JointStartupCalibration +from blue_msgs.msg import GravityVectorArray +from message_filters import TimeSynchronizer, Subscriber, ApproximateTimeSynchronizer +import numpy as np +from urdf_parser_py import urdf # http://docs.ros.org/indigo/api/message_filters/html/python/#message_filters.TimeSynchronizer # synch callbacks with this -def measurement_likelihood(vec_part, vec_meas): - return np.linalg.norm(vec_part.dot(vec_meas)) +def measurement_likelihood(v_part, v_meas): + part = np.array([v_part.x(), v_part.y(), v_part.z()]) + meas = np.array([v_meas.x, v_meas.y, v_meas.z]) + return np.abs(part.dot(meas)) class ParticleGroup: def __init__(self, num_joints, num_particles, joint_min, joint_max, fk): @@ -26,24 +32,42 @@ def __init__(self, num_joints, num_particles, joint_min, joint_max, fk): self.jmin = joint_min self.jmax = joint_max self.fk_solver = fk - self.particle_offsets = np.uniform.random(self.jmin, self.jmax, (num_p, num_j)) + # uniformly initalize potential positions from given max and min + self.particle_offsets = np.random.uniform(self.jmin, self.jmax, (self.num_p, self.num_j)) + rospy.logerr(self.particle_offsets) def update(self, joints, accel_1, accel_2): - # TODO figure out accel message type ml_array = [] + kdl_joints = kdl.JntArray(self.num_j) + frame = kdl.Frame() + + # for each particle (row joint offset) check the likelyhood given the measurement for p in self.particle_offsets: - frame = kdl.Frame() - self.fk_solver.JntToCart(joints + p, frame) + # get the joints angle with offset for the given particle + corrected_joints = np.array(joints) + p + for j in range(self.num_j): + kdl_joints[j] = corrected_joints[j] + self.fk_solver.JntToCart(kdl_joints, frame) + # find the frame transformation + grav_prev = kdl.Vector() - grav_prev.data = accel_1 + grav_prev.x(accel_1.x) + grav_prev.y(accel_1.y) + grav_prev.z(accel_1.z) + + # transform the gravity vector by the given frame grav_post = frame * grav_prev + + # append the measurement likelyhood ml = measurement_likelihood(grav_post, accel_2) ml_array.append(ml) + # normalize measurement likelyhood and resample particles alpha = np.sum(ml_array) ml_array = np.array(ml_array) / alpha cdf = np.cumsum(ml_array) + # TODO verify that this is done correctly self.particle_offsets = np.array([self.particle_offsets[np.argwhere(cdf>np.random.uniform())[0,0],:] for i in range(self.num_p)]) @@ -58,9 +82,12 @@ class BlueRobotFilter: def _setup(self): # load in ros parameters self.baselink = rospy.get_param("blue_hardware/baselink") - self.endlink = rospy.get_param("blue_harware/endlink") - flag, self.tree = kdl_parser.treeFromParam("/robot_description") + self.endlink = rospy.get_param("blue_hardware/endlink") + robot_description = rospy.get_param("/robot_description") + flag, self.tree = kdl_parser.treeFromString(robot_description) + # build kinematic chain and fk and jacobian solvers + self.robot_urdf = urdf.Robot.from_xml_string(robot_description) chain_ee = self.tree.getChain(self.baselink, self.endlink) # building robot joint state @@ -68,33 +95,46 @@ def _setup(self): # other joint information self.joint_names = rospy.get_param("blue_hardware/joint_names") - self.accel_links = rospy.get_param("blue_hardware/accel_links").insert(0, self.baselink) + self.accel_links = rospy.get_param("blue_hardware/accel_links") + self.differential_pairs = rospy.get_param("blue_hardware/differential_pairs") + + # load in min and max joints + self.jmin = [] + self.jmax = [] + for j in range(self.num_joints): + joint_limit = self.robot_urdf.joint_map[self.joint_names[j]].limit + self.jmin.append(joint_limit.lower) + self.jmax.append(joint_limit.upper) + + self.accel_links.insert(0, self.baselink) self.best_estimate = np.zeros(self.num_joints) self.probability = 0 # TODO make a rosparam - particles_per_joints = 100 + particles_per_joint = 100 ppj = particles_per_joint p_groups = [] joints_per_group = [] i = 0 g = 0 - while i < num_joints: - if i in differential_pairs: - chain = self.tree.getChain(self.accel_links[g], self.accel_links[g+1]) - fk = kdl.ChainFkSolverPos_recursive(chain) - pg = ParticleGroup(2, ppj, [jmin[i], jmin[i+1]], [jmax[i], jmax[i+1]], fk) + # build chain and filter between each gravity vector + while i < self.num_joints: + # get chain between two accel_links and build respective fk solver + chain = self.tree.getChain(self.accel_links[g], self.accel_links[g+1]) + rospy.logerr("pair: " + self.accel_links[g] +", " + self.accel_links[g+1]) + fk = kdl.ChainFkSolverPos_recursive(chain) + + # build particle groups + if i in self.differential_pairs: + pg = ParticleGroup(2, ppj, [self.jmin[i], self.jmin[i+1]], [self.jmax[i], self.jmax[i+1]], fk) p_groups.append(pg) joints_per_group.append(2) i = i + 1 else: - chain = self.tree.getChain(self.accel_links[g], self.accel_links[g+1]) - fk = kdl.ChainFkSolverPos_recursive(chain) - pg = ParticleGroup(1, ppj, [jmin[i]], [jmax[i]], fk) - + pg = ParticleGroup(1, ppj, [self.jmin[i]], [self.jmax[i]], fk) p_groups.append(pg) joints_per_group.append(1) i = i + 1 @@ -115,31 +155,41 @@ def get_probability(self): def get_best_estimate(self): return self.best_estimate - def update_filter(self, joints, accel): - new_joints = self.joints.copy() + def update_filter(self, joints_msg, accel_msg): + + # retrive joint angles in correct order from joint state message (comes unordered) + new_joint_positions = [] + for jn in self.joint_names: + index = joints_msg.name.index(jn) + new_joint_positions.append(joints_msg.position[index]) + i = 0 probabilities = [] best_estimate = [] for j, pg in enumerate(self.pg_array): - index = joint_msg.name.index(n) - self.joints[i] = joint_msg.position[index] + # for each particle filter group querry the relevent joint angles new_link_joints = [] for k in range(self.jpg[j]): - new_link_joints.append(joint_msg.position[i]) + new_link_joints.append(new_joint_positions[i]) i = i + 1 - joints, prob = pg.update(new_link_joints, self.accel_links[j], self.accel_links[j+1]) + # update the filter with new accelerometer values and joint angles + joints, prob = pg.update(new_link_joints, accel_msg.vectors[j], accel_msg.vectors[j+1]) probabilities.append(prob) best_estimate.append(joints) - self.best_estimate = np.array(best_estimate) + self.best_estimate = np.array([item for sublist in best_estimate for item in sublist]) self.probability = np.min(probabilities) + # rospy.logerr(best_estimate) + # rospy.logerr(probabilities) + def calibrate(self): - # TODO write this function which uses the synchronized callbacks - # rospy.Subscriber("/accel", Brents custom accel, self.update_joints) - rospy.Subscriber("/joint_states", JointState, self.update_joints) - # synch callbacks with this - # sych callback = update filter + joint_state_subscriber = Subscriber("/joint_states", JointState) + gravity_vector_subscriber = Subscriber("blue_hardware/gravity_vectors", GravityVectorArray) + tss = ApproximateTimeSynchronizer([joint_state_subscriber, gravity_vector_subscriber], queue_size=2, slop=0.05) + + # register joint states and gravity vectors callback + tss.registerCallback(self.update_filter) if __name__ == "__main__": rospy.init_node("simple_startup_calibration") @@ -147,7 +197,11 @@ def calibrate(self): # building the kinematic chains rospy.loginfo("Building Blue Object...") blue = BlueRobotFilter(debug=True) + + # start the calibration callback functions blue.calibrate() + + # wait until a high probability is reached r = rospy.Rate(1.0) while blue.get_probability() < 0.9: r.sleep(); From a23bdf06dc10467a403de5a65f97ef826320195a Mon Sep 17 00:00:00 2001 From: Philipp Date: Sat, 24 Nov 2018 17:09:31 -0800 Subject: [PATCH 09/12] debuging --- blue_bringup/launch/accel_test.launch | 10 ++++ blue_bringup/scripts/accel_calibration.py | 71 ++++++++++++++++++----- 2 files changed, 65 insertions(+), 16 deletions(-) diff --git a/blue_bringup/launch/accel_test.launch b/blue_bringup/launch/accel_test.launch index eaad8208..dd22579e 100644 --- a/blue_bringup/launch/accel_test.launch +++ b/blue_bringup/launch/accel_test.launch @@ -1,9 +1,19 @@ + + + + diff --git a/blue_bringup/scripts/accel_calibration.py b/blue_bringup/scripts/accel_calibration.py index f08a500f..4c93d811 100755 --- a/blue_bringup/scripts/accel_calibration.py +++ b/blue_bringup/scripts/accel_calibration.py @@ -3,6 +3,7 @@ """This node should run at startup, and sets the initial joint angles to some hardcoded values.""" import rospy +import pickle import sys import actionlib from std_msgs.msg import Int32 @@ -16,6 +17,7 @@ from message_filters import TimeSynchronizer, Subscriber, ApproximateTimeSynchronizer import numpy as np from urdf_parser_py import urdf +from scipy.stats import multivariate_normal # http://docs.ros.org/indigo/api/message_filters/html/python/#message_filters.TimeSynchronizer # synch callbacks with this @@ -25,6 +27,12 @@ def measurement_likelihood(v_part, v_meas): meas = np.array([v_meas.x, v_meas.y, v_meas.z]) return np.abs(part.dot(meas)) +# def measurement_likelihood(v_part, v_meas): + # part = np.array([v_part.x(), v_part.y(), v_part.z()]) + # meas = np.array([v_meas.x, v_meas.y, v_meas.z]) + # ml = multivariate_normal.pdf(part, mean=meas, cov=100.0*np.ones(3)) + # return ml + class ParticleGroup: def __init__(self, num_joints, num_particles, joint_min, joint_max, fk): self.num_j = num_joints @@ -34,10 +42,10 @@ def __init__(self, num_joints, num_particles, joint_min, joint_max, fk): self.fk_solver = fk # uniformly initalize potential positions from given max and min self.particle_offsets = np.random.uniform(self.jmin, self.jmax, (self.num_p, self.num_j)) - rospy.logerr(self.particle_offsets) + # rospy.logerr(self.particle_offsets) def update(self, joints, accel_1, accel_2): - ml_array = [] + measurement_likelihood_array = [] kdl_joints = kdl.JntArray(self.num_j) frame = kdl.Frame() @@ -60,27 +68,31 @@ def update(self, joints, accel_1, accel_2): # append the measurement likelyhood ml = measurement_likelihood(grav_post, accel_2) - ml_array.append(ml) + measurement_likelihood_array.append(ml) # normalize measurement likelyhood and resample particles - alpha = np.sum(ml_array) - ml_array = np.array(ml_array) / alpha - cdf = np.cumsum(ml_array) + alpha = np.sum(measurement_likelihood_array) + measurement_likelihood_array= np.array(measurement_likelihood_array) / alpha + cdf = np.cumsum(measurement_likelihood_array) # TODO verify that this is done correctly self.particle_offsets = np.array([self.particle_offsets[np.argwhere(cdf>np.random.uniform())[0,0],:] for i in range(self.num_p)]) # TODO implement roughening - if False: - pass - - index = np.argmax(ml_array) - return self.particle_offsets[index,:], ml_array[index] + if True: + mean = np.zeros(self.num_j) + sigma = 0.0001 * 2.0 * self.num_p**(-1.0/3.0) * np.ones((self.num_j, self.num_j)) + noise = np.random.multivariate_normal(mean, sigma, self.num_p) + self.particle_offsets = self.particle_offsets + noise + index = np.argmax(measurement_likelihood_array) + return self.particle_offsets[index,:], measurement_likelihood_array[index] class BlueRobotFilter: def _setup(self): # load in ros parameters + self.gt= np.array(rospy.get_param("blue_hardware/simple_startup_angles")) + self.baselink = rospy.get_param("blue_hardware/baselink") self.endlink = rospy.get_param("blue_hardware/endlink") robot_description = rospy.get_param("/robot_description") @@ -97,6 +109,7 @@ def _setup(self): self.joint_names = rospy.get_param("blue_hardware/joint_names") self.accel_links = rospy.get_param("blue_hardware/accel_links") self.differential_pairs = rospy.get_param("blue_hardware/differential_pairs") + rospy.logerr(self.joint_names) # load in min and max joints self.jmin = [] @@ -105,6 +118,8 @@ def _setup(self): joint_limit = self.robot_urdf.joint_map[self.joint_names[j]].limit self.jmin.append(joint_limit.lower) self.jmax.append(joint_limit.upper) + rospy.logerr(self.jmin) + rospy.logerr(self.jmax) self.accel_links.insert(0, self.baselink) @@ -112,7 +127,7 @@ def _setup(self): self.probability = 0 # TODO make a rosparam - particles_per_joint = 100 + particles_per_joint = 200 ppj = particles_per_joint p_groups = [] @@ -147,6 +162,7 @@ def __init__(self, debug=False): self.debug = debug if self.debug: self.debug_count = 0 + self.history = [] self._setup() def get_probability(self): @@ -156,7 +172,6 @@ def get_best_estimate(self): return self.best_estimate def update_filter(self, joints_msg, accel_msg): - # retrive joint angles in correct order from joint state message (comes unordered) new_joint_positions = [] for jn in self.joint_names: @@ -166,6 +181,7 @@ def update_filter(self, joints_msg, accel_msg): i = 0 probabilities = [] best_estimate = [] + for j, pg in enumerate(self.pg_array): # for each particle filter group querry the relevent joint angles new_link_joints = [] @@ -177,33 +193,56 @@ def update_filter(self, joints_msg, accel_msg): probabilities.append(prob) best_estimate.append(joints) + if self.debug: + self.debug_count += 1 + all_particles = [] + for pg in self.pg_array: + all_particles.append(pg.particle_offsets.copy()) + # rospy.logerr(all_particles) + all_particles = np.hstack(all_particles) + if self.debug_count&5 == 0: + # rospy.logerr(all_particles.shape) + rospy.logerr(len(self.history)) + self.history.append(all_particles) + if len(self.history) == 100: + with open('/home/phil/gauss.pickle', 'wb') as handle: + pickle.dump(self.history, handle) + self.best_estimate = np.array([item for sublist in best_estimate for item in sublist]) self.probability = np.min(probabilities) - # rospy.logerr(best_estimate) + rospy.logerr(np.array(self.best_estimate) - self.gt[0:7]) + # rospy.logerr(np.array(self.best_estimate) - np.array(new_joint_positions)[0:7]) # rospy.logerr(probabilities) def calibrate(self): joint_state_subscriber = Subscriber("/joint_states", JointState) gravity_vector_subscriber = Subscriber("blue_hardware/gravity_vectors", GravityVectorArray) - tss = ApproximateTimeSynchronizer([joint_state_subscriber, gravity_vector_subscriber], queue_size=2, slop=0.05) + tss = ApproximateTimeSynchronizer([joint_state_subscriber, gravity_vector_subscriber], queue_size=50, slop=0.01) # register joint states and gravity vectors callback tss.registerCallback(self.update_filter) +# + # def exit(self): + # if self.debug: + if __name__ == "__main__": + rospy.init_node("simple_startup_calibration") # building the kinematic chains rospy.loginfo("Building Blue Object...") blue = BlueRobotFilter(debug=True) + # atexit.register(blue.exit) # start the calibration callback functions blue.calibrate() # wait until a high probability is reached r = rospy.Rate(1.0) - while blue.get_probability() < 0.9: + start = rospy.get_rostime() + while blue.get_probability() < 0.9 and not rospy.is_shutdown() and start + rospy.Duration.from_sec(60) > rospy.get_rostime(): r.sleep(); # Read startup angles from parameter server From 7d5fdc1017765a6c3e4db2f84cce0827ac489bd5 Mon Sep 17 00:00:00 2001 From: Philipp Date: Sun, 25 Nov 2018 12:06:55 -0800 Subject: [PATCH 10/12] fixed accel calib bug --- blue_bringup/scripts/accel_calibration.py | 30 ++++++++++++----------- 1 file changed, 16 insertions(+), 14 deletions(-) diff --git a/blue_bringup/scripts/accel_calibration.py b/blue_bringup/scripts/accel_calibration.py index 4c93d811..95e2167f 100755 --- a/blue_bringup/scripts/accel_calibration.py +++ b/blue_bringup/scripts/accel_calibration.py @@ -22,16 +22,16 @@ # http://docs.ros.org/indigo/api/message_filters/html/python/#message_filters.TimeSynchronizer # synch callbacks with this -def measurement_likelihood(v_part, v_meas): - part = np.array([v_part.x(), v_part.y(), v_part.z()]) - meas = np.array([v_meas.x, v_meas.y, v_meas.z]) - return np.abs(part.dot(meas)) - # def measurement_likelihood(v_part, v_meas): # part = np.array([v_part.x(), v_part.y(), v_part.z()]) # meas = np.array([v_meas.x, v_meas.y, v_meas.z]) - # ml = multivariate_normal.pdf(part, mean=meas, cov=100.0*np.ones(3)) - # return ml + # return np.abs(part.dot(meas)) + +def measurement_likelihood(v_part, v_meas): + part = np.array([v_part.x(), v_part.y(), v_part.z()]) + meas = np.array([v_meas.x, v_meas.y, v_meas.z]) + ml = multivariate_normal.pdf(part, mean=meas, cov=100.0*np.ones(3)) + return ml class ParticleGroup: def __init__(self, num_joints, num_particles, joint_min, joint_max, fk): @@ -57,17 +57,19 @@ def update(self, joints, accel_1, accel_2): kdl_joints[j] = corrected_joints[j] self.fk_solver.JntToCart(kdl_joints, frame) # find the frame transformation + accel_base = accel_1 + accel_end = accel_2 grav_prev = kdl.Vector() - grav_prev.x(accel_1.x) - grav_prev.y(accel_1.y) - grav_prev.z(accel_1.z) + grav_prev.x(accel_2.x) + grav_prev.y(accel_2.y) + grav_prev.z(accel_2.z) # transform the gravity vector by the given frame grav_post = frame * grav_prev # append the measurement likelyhood - ml = measurement_likelihood(grav_post, accel_2) + ml = measurement_likelihood(grav_post, accel_base) measurement_likelihood_array.append(ml) @@ -82,7 +84,7 @@ def update(self, joints, accel_1, accel_2): # TODO implement roughening if True: mean = np.zeros(self.num_j) - sigma = 0.0001 * 2.0 * self.num_p**(-1.0/3.0) * np.ones((self.num_j, self.num_j)) + sigma = 0.0001 * 1.0 * self.num_p**(-1.0/3.0) * np.ones((self.num_j, self.num_j)) noise = np.random.multivariate_normal(mean, sigma, self.num_p) self.particle_offsets = self.particle_offsets + noise index = np.argmax(measurement_likelihood_array) @@ -204,8 +206,8 @@ def update_filter(self, joints_msg, accel_msg): # rospy.logerr(all_particles.shape) rospy.logerr(len(self.history)) self.history.append(all_particles) - if len(self.history) == 100: - with open('/home/phil/gauss.pickle', 'wb') as handle: + if len(self.history) == 200: + with open('/home/phil/fixed.pickle', 'wb') as handle: pickle.dump(self.history, handle) self.best_estimate = np.array([item for sublist in best_estimate for item in sublist]) From b3b8eeff46f8e322bde503b59907f185ba9db7b8 Mon Sep 17 00:00:00 2001 From: Philipp Date: Tue, 29 Jan 2019 18:14:43 -0800 Subject: [PATCH 11/12] small bug fix --- .../config/robot_parameters_left.yaml | 6 ++-- blue_bringup/launch/accel_test.launch | 4 +-- .../include/single_arm_control.launch.xml | 4 +-- blue_bringup/scripts/accel_calibration.py | 32 ++++++++++++++++--- 4 files changed, 35 insertions(+), 11 deletions(-) diff --git a/blue_bringup/config/robot_parameters_left.yaml b/blue_bringup/config/robot_parameters_left.yaml index 7dd2ac29..9beb1f03 100644 --- a/blue_bringup/config/robot_parameters_left.yaml +++ b/blue_bringup/config/robot_parameters_left.yaml @@ -13,9 +13,9 @@ blue_hardware: endlink: left_gripper_finger_link accel_links: - left_base_link - - left_shoulder_accel_link - - left_elbow_accel_link - - left_wrist_accel_link + - left_shoulder_roll_link + - left_elbow_roll_link + - left_wrist_roll_link - left_end_roll_link differential_pairs: [1, 2, 3, 4, 5, 6] gear_ratios: [7.1875, 7.1875, 8.2444852941, 7.1875, 8.24448529412, 7.1875, 8.24448529412, 14.4] diff --git a/blue_bringup/launch/accel_test.launch b/blue_bringup/launch/accel_test.launch index dd22579e..bf22306d 100644 --- a/blue_bringup/launch/accel_test.launch +++ b/blue_bringup/launch/accel_test.launch @@ -3,7 +3,7 @@ name="robot_description" command="$(find xacro)/xacro --inorder '$(find blue_descriptions)/robots/blue_full.urdf.xacro'" /> - + @@ -13,7 +13,7 @@ type="accel_calibration.py" output="screen" /> diff --git a/blue_bringup/launch/include/single_arm_control.launch.xml b/blue_bringup/launch/include/single_arm_control.launch.xml index 2e7eebda..4bc65659 100644 --- a/blue_bringup/launch/include/single_arm_control.launch.xml +++ b/blue_bringup/launch/include/single_arm_control.launch.xml @@ -45,9 +45,9 @@ - + - + diff --git a/blue_bringup/scripts/accel_calibration.py b/blue_bringup/scripts/accel_calibration.py index 95e2167f..c7db3aef 100755 --- a/blue_bringup/scripts/accel_calibration.py +++ b/blue_bringup/scripts/accel_calibration.py @@ -30,7 +30,7 @@ def measurement_likelihood(v_part, v_meas): part = np.array([v_part.x(), v_part.y(), v_part.z()]) meas = np.array([v_meas.x, v_meas.y, v_meas.z]) - ml = multivariate_normal.pdf(part, mean=meas, cov=100.0*np.ones(3)) + ml = multivariate_normal.pdf(part, mean=meas, cov=10.0*np.ones(3)) return ml class ParticleGroup: @@ -56,6 +56,7 @@ def update(self, joints, accel_1, accel_2): for j in range(self.num_j): kdl_joints[j] = corrected_joints[j] self.fk_solver.JntToCart(kdl_joints, frame) + # print(frame) # find the frame transformation accel_base = accel_1 accel_end = accel_2 @@ -84,7 +85,7 @@ def update(self, joints, accel_1, accel_2): # TODO implement roughening if True: mean = np.zeros(self.num_j) - sigma = 0.0001 * 1.0 * self.num_p**(-1.0/3.0) * np.ones((self.num_j, self.num_j)) + sigma = 1 * 0.0001 * self.num_p**(-1.0/3.0) * np.ones((self.num_j, self.num_j)) noise = np.random.multivariate_normal(mean, sigma, self.num_p) self.particle_offsets = self.particle_offsets + noise index = np.argmax(measurement_likelihood_array) @@ -124,12 +125,11 @@ def _setup(self): rospy.logerr(self.jmax) - self.accel_links.insert(0, self.baselink) self.best_estimate = np.zeros(self.num_joints) self.probability = 0 # TODO make a rosparam - particles_per_joint = 200 + particles_per_joint = 1000 ppj = particles_per_joint p_groups = [] @@ -160,6 +160,30 @@ def _setup(self): self.pg_array = p_groups self.jpg = joints_per_group + + if self.debug: + pg = self.pg_array[1] + # frame = kdl.Frame() + + kdl_joints = kdl.JntArray(2) + kdl_joints[0] = 0 + kdl_joints[1] = np.pi/2 + pg.fk_solver.JntToCart(kdl_joints, frame) + # print(frame) + # find the frame transformation + accel_base = np.array([0 , 0, 9.81]) + accel_end = np.array([0 , 0, 9.81]) + + grav_prev = kdl.Vector() + grav_prev.x(accel_end[0]) + grav_prev.y(accel_end[1]) + grav_prev.z(accel_end[2]) + + # transform the gravity vector by the given frame + grav_post = frame * grav_prev + # print("gravity post:= {}".format(grav_post)) + # print("gravity orig:= {}".format(accel_base)) + def __init__(self, debug=False): self.debug = debug if self.debug: From c1b61d4b7bd7c09e285181d2e27609379d4bbe1d Mon Sep 17 00:00:00 2001 From: Brent Yi Date: Mon, 4 Feb 2019 15:07:49 -0600 Subject: [PATCH 12/12] accel calibration working --- blue_bringup/scripts/accel_calibration.py | 30 +++++++++++------------ 1 file changed, 15 insertions(+), 15 deletions(-) diff --git a/blue_bringup/scripts/accel_calibration.py b/blue_bringup/scripts/accel_calibration.py index c7db3aef..1ce9de57 100755 --- a/blue_bringup/scripts/accel_calibration.py +++ b/blue_bringup/scripts/accel_calibration.py @@ -161,26 +161,26 @@ def _setup(self): self.jpg = joints_per_group - if self.debug: - pg = self.pg_array[1] + # if self.debug: + # pg = self.pg_array[1] # frame = kdl.Frame() - kdl_joints = kdl.JntArray(2) - kdl_joints[0] = 0 - kdl_joints[1] = np.pi/2 - pg.fk_solver.JntToCart(kdl_joints, frame) + # kdl_joints = kdl.JntArray(2) + # kdl_joints[0] = 0 + # kdl_joints[1] = np.pi/2 + # pg.fk_solver.JntToCart(kdl_joints, frame) # print(frame) # find the frame transformation - accel_base = np.array([0 , 0, 9.81]) - accel_end = np.array([0 , 0, 9.81]) - - grav_prev = kdl.Vector() - grav_prev.x(accel_end[0]) - grav_prev.y(accel_end[1]) - grav_prev.z(accel_end[2]) - + # accel_base = np.array([0 , 0, 9.81]) + # accel_end = np.array([0 , 0, 9.81]) +# + # grav_prev = kdl.Vector() + # grav_prev.x(accel_end[0]) + # grav_prev.y(accel_end[1]) + # grav_prev.z(accel_end[2]) +# # transform the gravity vector by the given frame - grav_post = frame * grav_prev + # grav_post = frame * grav_prev # print("gravity post:= {}".format(grav_post)) # print("gravity orig:= {}".format(accel_base))