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])
