本文整理汇总了Python中hrl_geom.pose_converter.PoseConv.to_pos_euler方法的典型用法代码示例。如果您正苦于以下问题：Python PoseConv.to_pos_euler方法的具体用法？Python PoseConv.to_pos_euler怎么用？Python PoseConv.to_pos_euler使用的例子？那么恭喜您, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在类hrl_geom.pose_converter.PoseConv的用法示例。
在下文中一共展示了PoseConv.to_pos_euler方法的3个代码示例，这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞，您的评价将有助于系统推荐出更棒的Python代码示例。
示例1: _run_traj
# 需要导入模块: from hrl_geom.pose_converter import PoseConv [as 别名]
# 或者: from hrl_geom.pose_converter.PoseConv import to_pos_euler [as 别名]
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)示例2: command_move_cb
# 需要导入模块: from hrl_geom.pose_converter import PoseConv [as 别名]
# 或者: from hrl_geom.pose_converter.PoseConv import to_pos_euler [as 别名]
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)示例3: main
# 需要导入模块: from hrl_geom.pose_converter import PoseConv [as 别名]
# 或者: from hrl_geom.pose_converter.PoseConv import to_pos_euler [as 别名]
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:
#.........这里部分代码省略.........