def command_move_cb(self, msg): if self.arm is None: rospy.logwarn( "[cartesian_manager] Cartesian controller not enabled.") return self.cart_ctrl.stop_moving(wait=True) if msg.header.frame_id == "": msg.header.frame_id = "torso_lift_link" if self.kin is None or msg.header.frame_id not in self.kin.get_link_names( ): self.kin = create_joint_kin("torso_lift_link", msg.header.frame_id) torso_pos_ep, torso_rot_ep = PoseConv.to_pos_rot(self.arm.get_ep()) torso_B_ref = self.kin.forward(base_link="torso_lift_link", end_link=msg.header.frame_id) _, torso_rot_ref = PoseConv.to_pos_rot(torso_B_ref) torso_rot_ref *= self.frame_rot ref_pos_off, ref_rot_off = PoseConv.to_pos_rot(msg) change_pos = torso_rot_ep.T * torso_rot_ref * ref_pos_off change_pos_xyz = change_pos.T.A[0] ep_rot_ref = torso_rot_ep.T * torso_rot_ref change_rot = ep_rot_ref * ref_rot_off * ep_rot_ref.T _, change_rot_rpy = PoseConv.to_pos_euler( np.mat([0] * 3).T, change_rot) rospy.loginfo("[cartesian_manager] Command relative movement." + str((change_pos_xyz, change_rot_rpy))) self.cart_ctrl.execute_cart_move((change_pos_xyz, change_rot_rpy), ((0, 0, 0), 0), velocity=self.velocity, blocking=True)
def _run_traj(self, traj, blocking=True): self.start_pub.publish( PoseConv.to_pose_stamped_msg("/torso_lift_link", traj[0])) self.end_pub.publish( PoseConv.to_pose_stamped_msg("/torso_lift_link", traj[-1])) # make sure traj beginning is close to current end effector position init_pos_tolerance = rospy.get_param("~init_pos_tolerance", 0.05) init_rot_tolerance = rospy.get_param("~init_rot_tolerance", np.pi/12) ee_pos, ee_rot = PoseConv.to_pos_rot(self.arm.get_end_effector_pose()) _, rot_diff = PoseConv.to_pos_euler((ee_pos, ee_rot * traj[0][1].T)) pos_diff = np.linalg.norm(ee_pos - traj[0][0]) if pos_diff > init_pos_tolerance: rospy.logwarn("[controller_base] End effector too far from current position. " + "Pos diff: %.3f, Tolerance: %.3f" % (pos_diff, init_pos_tolerance)) return False if np.linalg.norm(rot_diff) > init_rot_tolerance: rospy.logwarn("[controller_base] End effector too far from current rotation. " + "Rot diff: %.3f, Tolerance: %.3f" % (np.linalg.norm(rot_diff), init_rot_tolerance)) return False return self.execute_cart_traj(self.arm, traj, self.time_step, blocking=blocking)
def command_move_cb(self, msg): if self.arm is None: rospy.logwarn("[cartesian_manager] Cartesian controller not enabled.") return self.cart_ctrl.stop_moving(wait=True) if msg.header.frame_id == "": msg.header.frame_id = "torso_lift_link" if self.kin is None or msg.header.frame_id not in self.kin.get_link_names(): self.kin = create_joint_kin("torso_lift_link", msg.header.frame_id) torso_pos_ep, torso_rot_ep = PoseConv.to_pos_rot(self.arm.get_ep()) torso_B_ref = self.kin.forward(base_link="torso_lift_link", end_link=msg.header.frame_id) _, torso_rot_ref = PoseConv.to_pos_rot(torso_B_ref) torso_rot_ref *= self.frame_rot ref_pos_off, ref_rot_off = PoseConv.to_pos_rot(msg) change_pos = torso_rot_ep.T * torso_rot_ref * ref_pos_off change_pos_xyz = change_pos.T.A[0] ep_rot_ref = torso_rot_ep.T * torso_rot_ref change_rot = ep_rot_ref * ref_rot_off * ep_rot_ref.T _, change_rot_rpy = PoseConv.to_pos_euler(np.mat([0]*3).T, change_rot) rospy.loginfo("[cartesian_manager] Command relative movement." + str((change_pos_xyz, change_rot_rpy))) self.cart_ctrl.execute_cart_move((change_pos_xyz, change_rot_rpy), ((0, 0, 0), 0), velocity=self.velocity, blocking=True)
# y: 0.0 # z: 0.0 homo_mat = PoseConv.to_homo_mat(twist_msg) print homo_mat #[[ 1. 0. 0. 0.1 ] # [ 0. 0.95533649 -0.29552021 0.2 ] # [-0. 0.29552021 0.95533649 0.3 ] # [ 0. 0. 0. 1. ]] [pos, rot] = homo_mat[:3, 3], homo_mat[:3, :3] tf_stamped_msg = PoseConv.to_tf_stamped_msg("base_link", pos, rot) print tf_stamped_msg #header: # seq: 0 # stamp: # secs: 1341611677 # nsecs: 579762935 # frame_id: base_link #child_frame_id: '' #transform: # translation: # x: 0.1 # y: 0.2 # z: 0.3 # rotation: # x: 0.149438132474 # y: 0.0 # z: 0.0 # w: 0.988771077936 new_pos_euler = PoseConv.to_pos_euler(tf_stamped_msg) print new_pos_euler #([0.10000000000000001, 0.20000000000000001, 0.29999999999999999], (0.30000000000000004, -0.0, 0.0))
def main(): if True: #q = [ 4.07545758, 5.71643082, -4.57552159, -2.79061482, -3.17069678, 1.42865389] d1, a2, a3, d4, d5, d6 = [ 0.1273, -0.612, -0.5723, 0.163941, 0.1157, 0.0922 ] a = [0, a2, a3, 0, 0, 0] d = [d1, 0, 0, d4, d5, d6] l = [pi / 2, 0, 0, pi / 2, -pi / 2, 0] kin = RAVEKinematics() rospy.init_node("test_ur_ik") start_time = rospy.get_time() n = 0 while not rospy.is_shutdown(): q = (np.random.rand(6) - .5) * 4 * pi x1 = kin.forward(q) pos, euler = PoseConv.to_pos_euler(x1) m = np.random.randint(-4, 5) euler = [euler[0], m * np.pi / 2 + 0., euler[2]] #euler = [euler[0], 0.*np.pi/2 + m*np.pi, euler[2]] T = PoseConv.to_homo_mat(pos, euler) #q[4] = 0. T = kin.forward(q) sols = inverse_kin(T, a, d, l) print m, len(sols) if False and len(sols) == 0: print 'wuh', T sols = inverse_kin(T, a, d, l, True) if True: for qsol in sols: #Tsol, _ = forward_kin(qsol, a, d, l) Tsol = kin.forward(qsol) #print qsol #print q #print Tsol #print T diff = Tsol**-1 * T ang, _, _ = mat_to_ang_axis_point(diff) dist = np.linalg.norm(diff[:3, 3]) #print ang, dist if abs(dist) > 1e-5 or abs(ang) > 1e-5: print 'BAD' else: pass #print 'GOOD' n += 1 time_diff = rospy.get_time() - start_time print time_diff, n, n / time_diff if False: #q = [ 4.07545758, 5.71643082, -4.57552159, -2.79061482, -3.17069678, 1.42865389] d1, a2, a3, d4, d5, d6 = [ 0.1273, -0.612, -0.5723, 0.163941, 0.1157, 0.0922 ] a = [0, a2, a3, 0, 0, 0] d = [d1, 0, 0, d4, d5, d6] l = [pi / 2, 0, 0, pi / 2, -pi / 2, 0] kin = RAVEKinematics() rospy.init_node("test_ur_ik") start_time = rospy.get_time() n = 0 while not rospy.is_shutdown(): q = (np.random.rand(6) - .5) * 4 * pi T = kin.forward(q) sols = inverse_kin(T, a, d, l) #print len(sols) if False: print len(sols) for qsol in sols: #Tsol, _ = forward_kin(qsol, a, d, l) Tsol = kin.forward(qsol) diff = Tsol**-1 * T ang, _, _ = mat_to_ang_axis_point(diff) dist = np.linalg.norm(diff[:3, 3]) #print ang, dist if abs(dist) > 1e-8 or abs(ang) > 1e-8: print 'BAD' n += 1 time_diff = rospy.get_time() - start_time print time_diff, n, n / time_diff if False: #q = [ 4.07545758, 5.71643082, -4.57552159, -2.79061482, -3.17069678, 1.42865389] q = (np.random.rand(6) - .5) * 4 * pi d1, a2, a3, d4, d5, d6 = [ 0.1273, -0.612, -0.5723, 0.163941, 0.1157, 0.0922 ] a = [0, a2, a3, 0, 0, 0] d = [d1, 0, 0, d4, d5, d6] l = [pi / 2, 0, 0, pi / 2, -pi / 2, 0] kin = RAVEKinematics() T = kin.forward(q) print T print forward_kin(q, a, d, l) print q sols = inverse_kin(T, a, d, l) for qsol in sols: Tsol, _ = forward_kin(qsol, a, d, l) diff = Tsol**-1 * T ang, _, _ = mat_to_ang_axis_point(diff) dist = np.linalg.norm(diff[:3, 3]) if False: if abs(dist) > 1e-6: print '-' * 80 else: print '+' * 80 print 'T', T print 'qsol', qsol print 'q234', np.sum(qsol[1:4]) print 'q5', qsol[4] print '-sin(q2 + q3 + q4)*sin(q5)', -sin(qsol[1] + qsol[2] + qsol[3]) * sin( qsol[4]) print 'z3', T[2, 0] if abs(dist) > 1e-6: print '-' * 80 else: print '+' * 80 print ang, dist print np.sort(sols, 0) print len(sols) #unique_sols = np.array(sols)[np.where(np.hstack(([True], np.sum(np.diff(np.sort(sols,0), 1, 0),1) > 0.000)))[0]] #print unique_sols #print len(unique_sols) #print len(np.hstack(([True], np.sum(np.diff(np.sort(sols,0), 1, 0),1) > 0.000))) for qsol in sols: #Tsol, _ = forward_kin(qsol, a, d, l) Tsol = kin.forward(qsol) diff = Tsol**-1 * T ang, _, _ = mat_to_ang_axis_point(diff) dist = np.linalg.norm(diff[:3, 3]) print ang, dist if abs(dist) > 1e-8: print 'BAD' kin.robot.SetDOFValues(np.array([0.] * 6)) rave_sols = kin.manip.FindIKSolutions(T.A, kin.ik_options) rave_list = [] for qsol in rave_sols: rave_list.append(np.array(qsol)) #Tsol, _ = forward_kin(qsol, a, d, l) Tsol = kin.forward(qsol) diff = Tsol**-1 * T ang, _, _ = mat_to_ang_axis_point(diff) dist = np.linalg.norm(diff[:3, 3]) print ang, dist if abs(dist) > 1e-8: print 'BAD' print np.sort(rave_list, 0) #print diff #print q #print qsol #print '-'*80 if False: q = (np.random.rand(3) - 0.5) * 4. * np.pi T04 = forward_rrr(q) print T04 qs = inverse_rrr(T04) print qs print T04**-1 * forward_rrr(qs[0]) print T04**-1 * forward_rrr(qs[1])
def main(): if True: #q = [ 4.07545758, 5.71643082, -4.57552159, -2.79061482, -3.17069678, 1.42865389] d1, a2, a3, d4, d5, d6 = [0.1273, -0.612, -0.5723, 0.163941, 0.1157, 0.0922] a = [0, a2, a3, 0, 0, 0] d = [d1, 0, 0, d4, d5, d6] l = [pi/2, 0, 0, pi/2, -pi/2, 0] kin = RAVEKinematics() rospy.init_node("test_ur_ik") start_time = rospy.get_time() n = 0 while not rospy.is_shutdown(): q = (np.random.rand(6)-.5)*4*pi x1 = kin.forward(q) pos, euler = PoseConv.to_pos_euler(x1) m = np.random.randint(-4,5) euler = [euler[0], m*np.pi/2 + 0., euler[2]] #euler = [euler[0], 0.*np.pi/2 + m*np.pi, euler[2]] T = PoseConv.to_homo_mat(pos, euler) #q[4] = 0. T = kin.forward(q) sols = inverse_kin(T,a,d,l) print m, len(sols) if False and len(sols) == 0: print 'wuh', T sols = inverse_kin(T,a,d,l,True) if True: for qsol in sols: #Tsol, _ = forward_kin(qsol, a, d, l) Tsol = kin.forward(qsol) #print qsol #print q #print Tsol #print T diff = Tsol**-1 * T ang, _, _ = mat_to_ang_axis_point(diff) dist = np.linalg.norm(diff[:3,3]) #print ang, dist if abs(dist) > 1e-5 or abs(ang) > 1e-5: print 'BAD' else: pass #print 'GOOD' n += 1 time_diff = rospy.get_time() - start_time print time_diff, n, n/time_diff if False: #q = [ 4.07545758, 5.71643082, -4.57552159, -2.79061482, -3.17069678, 1.42865389] d1, a2, a3, d4, d5, d6 = [0.1273, -0.612, -0.5723, 0.163941, 0.1157, 0.0922] a = [0, a2, a3, 0, 0, 0] d = [d1, 0, 0, d4, d5, d6] l = [pi/2, 0, 0, pi/2, -pi/2, 0] kin = RAVEKinematics() rospy.init_node("test_ur_ik") start_time = rospy.get_time() n = 0 while not rospy.is_shutdown(): q = (np.random.rand(6)-.5)*4*pi T = kin.forward(q) sols = inverse_kin(T,a,d,l) #print len(sols) if False: print len(sols) for qsol in sols: #Tsol, _ = forward_kin(qsol, a, d, l) Tsol = kin.forward(qsol) diff = Tsol**-1 * T ang, _, _ = mat_to_ang_axis_point(diff) dist = np.linalg.norm(diff[:3,3]) #print ang, dist if abs(dist) > 1e-8 or abs(ang) > 1e-8: print 'BAD' n += 1 time_diff = rospy.get_time() - start_time print time_diff, n, n/time_diff if False: #q = [ 4.07545758, 5.71643082, -4.57552159, -2.79061482, -3.17069678, 1.42865389] q = (np.random.rand(6)-.5)*4*pi d1, a2, a3, d4, d5, d6 = [0.1273, -0.612, -0.5723, 0.163941, 0.1157, 0.0922] a = [0, a2, a3, 0, 0, 0] d = [d1, 0, 0, d4, d5, d6] l = [pi/2, 0, 0, pi/2, -pi/2, 0] kin = RAVEKinematics() T = kin.forward(q) print T print forward_kin(q,a,d,l) print q sols = inverse_kin(T,a,d,l) for qsol in sols: Tsol, _ = forward_kin(qsol, a, d, l) diff = Tsol**-1 * T ang, _, _ = mat_to_ang_axis_point(diff) dist = np.linalg.norm(diff[:3,3]) if False: if abs(dist) > 1e-6: print '-'*80 else: print '+'*80 print 'T', T print 'qsol', qsol print 'q234', np.sum(qsol[1:4]) print 'q5', qsol[4] print '-sin(q2 + q3 + q4)*sin(q5)', -sin(qsol[1] + qsol[2] + qsol[3])*sin(qsol[4]) print 'z3', T[2,0] if abs(dist) > 1e-6: print '-'*80 else: print '+'*80 print ang, dist print np.sort(sols,0) print len(sols) #unique_sols = np.array(sols)[np.where(np.hstack(([True], np.sum(np.diff(np.sort(sols,0), 1, 0),1) > 0.000)))[0]] #print unique_sols #print len(unique_sols) #print len(np.hstack(([True], np.sum(np.diff(np.sort(sols,0), 1, 0),1) > 0.000))) for qsol in sols: #Tsol, _ = forward_kin(qsol, a, d, l) Tsol = kin.forward(qsol) diff = Tsol**-1 * T ang, _, _ = mat_to_ang_axis_point(diff) dist = np.linalg.norm(diff[:3,3]) print ang, dist if abs(dist) > 1e-8: print 'BAD' kin.robot.SetDOFValues(np.array([0.]*6)) rave_sols = kin.manip.FindIKSolutions(T.A, kin.ik_options) rave_list = [] for qsol in rave_sols: rave_list.append(np.array(qsol)) #Tsol, _ = forward_kin(qsol, a, d, l) Tsol = kin.forward(qsol) diff = Tsol**-1 * T ang, _, _ = mat_to_ang_axis_point(diff) dist = np.linalg.norm(diff[:3,3]) print ang, dist if abs(dist) > 1e-8: print 'BAD' print np.sort(rave_list,0) #print diff #print q #print qsol #print '-'*80 if False: q = (np.random.rand(3)-0.5)*4.*np.pi T04 = forward_rrr(q) print T04 qs = inverse_rrr(T04) print qs print T04**-1 * forward_rrr(qs[0]) print T04**-1 * forward_rrr(qs[1])