本文整理汇总了Python中hrl_geom.pose_converter.PoseConv.to_pos_rot方法的典型用法代码示例。如果您正苦于以下问题:Python PoseConv.to_pos_rot方法的具体用法?Python PoseConv.to_pos_rot怎么用?Python PoseConv.to_pos_rot使用的例子?那么恭喜您, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在类hrl_geom.pose_converter.PoseConv
的用法示例。
在下文中一共展示了PoseConv.to_pos_rot方法的15个代码示例,这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞,您的评价将有助于系统推荐出更棒的Python代码示例。
示例1: cart_error
# 需要导入模块: from hrl_geom.pose_converter import PoseConv [as 别名]
# 或者: from hrl_geom.pose_converter.PoseConv import to_pos_rot [as 别名]
def cart_error(self, ep_actual, ep_desired):
pos_act, rot_act = PoseConv.to_pos_rot(ep_actual)
pos_des, rot_des = PoseConv.to_pos_rot(ep_desired)
err = np.mat(np.zeros((6, 1)))
err[:3,0] = pos_act - pos_des
err[3:6,0] = np.mat(trans.euler_from_matrix(rot_des.T * rot_act)).T
return err
示例2: inverse
# 需要导入模块: from hrl_geom.pose_converter import PoseConv [as 别名]
# 或者: from hrl_geom.pose_converter.PoseConv import to_pos_rot [as 别名]
def inverse(self, pose, q_guess=None, min_joints=None, max_joints=None):
pos, rot = PoseConv.to_pos_rot(pose)
pos_kdl = kdl.Vector(pos[0,0], pos[1,0], pos[2,0])
rot_kdl = kdl.Rotation(rot[0,0], rot[0,1], rot[0,2],
rot[1,0], rot[1,1], rot[1,2],
rot[2,0], rot[2,1], rot[2,2])
frame_kdl = kdl.Frame(rot_kdl, pos_kdl)
if min_joints is None:
min_joints = self.joint_safety_lower
if max_joints is None:
max_joints = self.joint_safety_upper
mins_kdl = joint_list_to_kdl(min_joints)
maxs_kdl = joint_list_to_kdl(max_joints)
ik_p_kdl = kdl.ChainIkSolverPos_NR_JL(self.chain, mins_kdl, maxs_kdl,
self._fk_kdl, self._ik_v_kdl)
if q_guess == None:
# use the midpoint of the joint limits as the guess
lower_lim = np.where(np.isfinite(min_joints), min_joints, 0.)
upper_lim = np.where(np.isfinite(max_joints), max_joints, 0.)
q_guess = (lower_lim + upper_lim) / 2.0
q_guess = np.where(np.isnan(q_guess), [0.]*len(q_guess), q_guess)
q_kdl = kdl.JntArray(self.num_joints)
q_guess_kdl = joint_list_to_kdl(q_guess)
if ik_p_kdl.CartToJnt(q_guess_kdl, frame_kdl, q_kdl) >= 0:
return np.array(joint_kdl_to_list(q_kdl))
else:
return None
示例3: robot_ellipsoidal_pose
# 需要导入模块: from hrl_geom.pose_converter import PoseConv [as 别名]
# 或者: from hrl_geom.pose_converter.PoseConv import to_pos_rot [as 别名]
def robot_ellipsoidal_pose(self, lat, lon, height, orient_quat, kinect_frame_mat=None):
if kinect_frame_mat is None:
kinect_frame_mat = self.get_ell_frame()
pos, quat = self.ell_space.ellipsoidal_to_pose(lat, lon, height)
quat_rotated = trans.quaternion_multiply(quat, orient_quat)
ell_pose_mat = PoseConv.to_homo_mat(pos, quat_rotated)
return PoseConv.to_pos_rot(kinect_frame_mat * ell_pose_mat)
示例4: FK
# 需要导入模块: from hrl_geom.pose_converter import PoseConv [as 别名]
# 或者: from hrl_geom.pose_converter.PoseConv import to_pos_rot [as 别名]
def FK(self, q, link_number=None):
if link_number is not None:
end_link = self.get_link_names(fixed=False)[link_number]
else:
end_link = None
homo_mat = self.forward(q, end_link)
pos, rot = PoseConv.to_pos_rot(homo_mat)
return pos, rot
示例5: enable_controller_cb
# 需要导入模块: from hrl_geom.pose_converter import PoseConv [as 别名]
# 或者: from hrl_geom.pose_converter.PoseConv import to_pos_rot [as 别名]
def enable_controller_cb(req):
if req.enable:
_, frame_rot = PoseConv.to_pos_rot([0]*3,
[req.frame_rot.x, req.frame_rot.y, req.frame_rot.z])
if req.velocity == 0:
req.velocity = 0.03
success = self.enable_controller(req.end_link, req.ctrl_params, req.ctrl_name,
frame_rot, velocity=req.velocity)
else:
success = self.disable_controller()
return EnableCartControllerResponse(success)
示例6: _ctrl_state_cb
# 需要导入模块: from hrl_geom.pose_converter import PoseConv [as 别名]
# 或者: from hrl_geom.pose_converter.PoseConv import to_pos_rot [as 别名]
def _ctrl_state_cb(self, ctrl_state):
self._save_ep(PoseConv.to_homo_mat(ctrl_state.x_desi_filtered))
with self.ctrl_state_lock:
self.ctrl_state_dict["frame"] = ctrl_state.header.frame_id
self.ctrl_state_dict["x_desi"] = PoseConv.to_pos_rot(ctrl_state.x_desi)
self.ctrl_state_dict["xd_desi"] = extract_twist(ctrl_state.xd_desi)
self.ctrl_state_dict["x_act"] = PoseConv.to_pos_rot(ctrl_state.x)
self.ctrl_state_dict["xd_act"] = extract_twist(ctrl_state.xd)
self.ctrl_state_dict["x_desi_filt"] = PoseConv.to_pos_rot(
ctrl_state.x_desi_filtered)
self.ctrl_state_dict["x_err"] = extract_twist(ctrl_state.x_err)
self.ctrl_state_dict["tau_pose"] = np.array(ctrl_state.tau_pose)
self.ctrl_state_dict["tau_posture"] = np.array(ctrl_state.tau_posture)
self.ctrl_state_dict["tau"] = np.array(ctrl_state.tau)
self.ctrl_state_dict["F"] = np.array([ctrl_state.F.force.x,
ctrl_state.F.force.y,
ctrl_state.F.force.z,
ctrl_state.F.torque.x,
ctrl_state.F.torque.y,
ctrl_state.F.torque.z])
示例7: execute_cart_move
# 需要导入模块: from hrl_geom.pose_converter import PoseConv [as 别名]
# 或者: from hrl_geom.pose_converter.PoseConv import to_pos_rot [as 别名]
def execute_cart_move(self, change_ep, abs_sel, velocity=0.001,
num_samps=None, blocking=True):
if not self.cmd_lock.acquire(False):
return False
cur_pos, cur_rot = PoseConv.to_pos_rot(self.arm.get_ep())
change_pos_ep, change_rot_ep = change_ep
abs_cart_ep_sel, is_abs_rot = abs_sel
pos_f = np.where(abs_cart_ep_sel, change_pos_ep,
np.array(cur_pos + cur_rot * np.mat(change_pos_ep).T).T[0])
if is_abs_rot:
rot_mat_f = change_rot_ep
else:
rpy = change_rot_ep
_, cur_rot = PoseConv.to_pos_rot(self.arm.get_ep())
rot_mat = np.mat(trans.euler_matrix(*rpy))[:3,:3]
rot_mat_f = cur_rot * rot_mat
traj = self._create_cart_trajectory(pos_f, rot_mat_f, velocity, num_samps)
retval = self._run_traj(traj, blocking=blocking)
self.cmd_lock.release()
return retval
示例8: create_tool_arrow
# 需要导入模块: from hrl_geom.pose_converter import PoseConv [as 别名]
# 或者: from hrl_geom.pose_converter.PoseConv import to_pos_rot [as 别名]
def create_tool_arrow():
arrows = MarkerArray()
color = ColorRGBA(0., 0., 1., 1.)
for i, param in enumerate(params):
ell_pos, ell_rot = params[param]
_, ell_rot_mat = PoseConv.to_pos_rot([0]*3, ell_rot)
cart_pose = PoseConv.to_homo_mat(ell_space.ellipsoidal_to_pose(*ell_pos))
cart_pose[:3,:3] = cart_pose[:3,:3] * ell_rot_mat
arrow = create_arrow_marker(cart_pose, i, color)
arrow.header.stamp = rospy.Time.now()
arrows.markers.append(arrow)
return arrows
示例9: create_ellipsoidal_path
# 需要导入模块: from hrl_geom.pose_converter import PoseConv [as 别名]
# 或者: from hrl_geom.pose_converter.PoseConv import to_pos_rot [as 别名]
def create_ellipsoidal_path(self, start_ell_pos, start_ell_quat,
end_ell_pos, end_ell_quat,
velocity=0.001, min_jerk=True):
print "Start rot (%s):\r\n%s" %(type(start_ell_quat),start_ell_quat)
print "End rot (%s):\r\n%s" %(type(end_ell_quat),end_ell_quat)
_, start_ell_rot = PoseConv.to_pos_rot((start_ell_pos,start_ell_quat))
_, end_ell_rot = PoseConv.to_pos_rot((end_ell_pos,end_ell_quat))
rpy = trans.euler_from_matrix(start_ell_rot.T * end_ell_rot) # get roll, pitch, yaw of angle diff
end_ell_pos[1] = np.mod(end_ell_pos[1], 2 * np.pi) # wrap longitude value
ell_init = np.mat(start_ell_pos).T
ell_final = np.mat(end_ell_pos).T
# find the closest longitude angle to interpolate to
if np.fabs(2 * np.pi + ell_final[1,0] - ell_init[1,0]) < np.pi:
ell_final[1,0] += 2 * np.pi
elif np.fabs(-2 * np.pi + ell_final[1,0] - ell_init[1,0]) < np.pi:
ell_final[1,0] -= 2 * np.pi
if np.any(np.isnan(ell_init)) or np.any(np.isnan(ell_final)):
rospy.logerr("[ellipsoid_space] Nan values in ellipsoid. " +
"ell_init: %f, %f, %f; " % (ell_init[0,0], ell_init[1,0], ell_init[2,0]) +
"ell_final: %f, %f, %f; " % (ell_final[0,0], ell_final[1,0], ell_final[2,0]))
return None
num_samps = np.max([2, int(np.linalg.norm(ell_final - ell_init) / velocity),
int(np.linalg.norm(rpy) / velocity)])
if min_jerk:
t_vals = min_jerk_traj(num_samps)
else:
t_vals = np.linspace(0,1,num_samps)
# smoothly interpolate from init to final
ell_lat_traj = np.interp(t_vals, (0,1),(start_ell_pos[0], end_ell_pos[0]))
ell_lon_traj = np.interp(t_vals, (0,1),(start_ell_pos[1], end_ell_pos[1]))
ell_height_traj = np.interp(t_vals, (0,1),(start_ell_pos[2], end_ell_pos[2]))
ell_pos_traj = np.vstack((ell_lat_traj, ell_lon_traj, ell_height_traj))
ell_quat_traj = [trans.quaternion_slerp(start_ell_quat, end_ell_quat, t) for t in t_vals]
return [(ell_pos_traj[:,i], ell_quat_traj[i]) for i in xrange(num_samps)]
示例10: _create_cart_trajectory
# 需要导入模块: from hrl_geom.pose_converter import PoseConv [as 别名]
# 或者: from hrl_geom.pose_converter.PoseConv import to_pos_rot [as 别名]
def _create_cart_trajectory(self, pos_f, rot_mat_f, velocity=0.001, num_samps=None):
cur_pos, cur_rot = PoseConv.to_pos_rot(self.arm.get_ep())
rpy = trans.euler_from_matrix(cur_rot.T * rot_mat_f) # get roll, pitch, yaw of angle diff
if num_samps is None:
num_samps = np.max([2, int(np.linalg.norm(pos_f - cur_pos) / velocity),
int(np.linalg.norm(rpy) / velocity)])
traj = self.arm.interpolate_ep([cur_pos, cur_rot],
[np.mat(pos_f).T, rot_mat_f],
min_jerk_traj(num_samps))
return traj
示例11: command_move_cb
# 需要导入模块: from hrl_geom.pose_converter import PoseConv [as 别名]
# 或者: from hrl_geom.pose_converter.PoseConv import to_pos_rot [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)
示例12: _create_ell_trajectory
# 需要导入模块: from hrl_geom.pose_converter import PoseConv [as 别名]
# 或者: from hrl_geom.pose_converter.PoseConv import to_pos_rot [as 别名]
def _create_ell_trajectory(self, ell_f, rot_mat_f, orient_quat=[0., 0., 0., 1.], velocity=0.001):
_, cur_rot = PoseConv.to_pos_rot(self.arm.get_ep())
rpy = trans.euler_from_matrix(cur_rot.T * rot_mat_f) # get roll, pitch, yaw of angle diff
ell_f[1] = np.mod(ell_f[1], 2 * np.pi) # wrap longitude value
# get the current ellipsoidal location of the end effector
ell_init = np.mat(self.get_ell_ep()).T
ell_final = np.mat(ell_f).T
# find the closest longitude angle to interpolate to
if np.fabs(2 * np.pi + ell_final[1,0] - ell_init[1,0]) < np.pi:
ell_final[1,0] += 2 * np.pi
elif np.fabs(-2 * np.pi + ell_final[1,0] - ell_init[1,0]) < np.pi:
ell_final[1,0] -= 2 * np.pi
if np.any(np.isnan(ell_init)) or np.any(np.isnan(ell_final)):
rospy.logerr("[ellipsoid_controller] Nan values in ellipsoid EPs. " +
"ell_init: %f, %f, %f; " % (ell_init[0,0], ell_init[1,0], ell_init[2,0]) +
"ell_final: %f, %f, %f; " % (ell_final[0,0], ell_final[1,0], ell_final[2,0]))
return None
num_samps = np.max([2, int(np.linalg.norm(ell_final - ell_init) / velocity),
int(np.linalg.norm(rpy) / velocity)])
t_vals = min_jerk_traj(num_samps) # makes movement smooth
# smoothly interpolate from init to final
ell_traj = np.array(ell_init) + np.array(np.tile(ell_final - ell_init,
(1, num_samps))) * np.array(t_vals)
ell_frame_mat = self.ell_server.get_ell_frame()
ell_pose_traj = [self.ell_server.robot_ellipsoidal_pose(
ell_traj[0,i], ell_traj[1,i], ell_traj[2,i], orient_quat, ell_frame_mat)
for i in range(ell_traj.shape[1])]
# modify rotation of trajectory
_, ell_init_rot = self.ell_server.robot_ellipsoidal_pose(
ell_init[0,0], ell_init[1,0], ell_init[2,0], orient_quat, ell_frame_mat)
rot_adjust_traj = self.arm.interpolate_ep([np.mat([0]*3).T, cur_rot],
[np.mat([0]*3).T, rot_mat_f],
min_jerk_traj(num_samps))
ell_pose_traj = [(ell_pose_traj[i][0],
ell_pose_traj[i][1] * ell_init_rot.T * rot_adjust_traj[i][1])
for i in range(num_samps)]
return ell_pose_traj
示例13: _run_traj
# 需要导入模块: from hrl_geom.pose_converter import PoseConv [as 别名]
# 或者: from hrl_geom.pose_converter.PoseConv import to_pos_rot [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)
示例14: command_absolute_cb
# 需要导入模块: from hrl_geom.pose_converter import PoseConv [as 别名]
# 或者: from hrl_geom.pose_converter.PoseConv import to_pos_rot [as 别名]
def command_absolute_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 = self.arm.get_ep()
torso_B_ref = self.kin.forward(base_link="torso_lift_link",
end_link=msg.header.frame_id)
ref_B_goal = PoseConv.to_homo_mat(msg)
torso_B_goal = torso_B_ref * ref_B_goal
change_pos, change_rot = PoseConv.to_pos_rot(torso_B_goal)
change_pos_xyz = change_pos.T.A[0]
rospy.loginfo("[cartesian_manager] Command absolute movement." +
str((change_pos_xyz, change_rot)))
self.cart_ctrl.execute_cart_move((change_pos_xyz, change_rot), ((1, 1, 1), 1),
velocity=self.velocity, blocking=True)
示例15: inverse_biased
# 需要导入模块: from hrl_geom.pose_converter import PoseConv [as 别名]
# 或者: from hrl_geom.pose_converter.PoseConv import to_pos_rot [as 别名]
def inverse_biased(self, pose, q_init, q_bias, q_bias_weights, rot_weight=1.0, bias_vel=0.01, num_iter=100):
# This code is potentially volitile
q_out = np.mat(self.inverse_search(pose)).T
for i in range(num_iter):
pos_fk, rot_fk = PoseConv.to_pos_rot(self.forward(q_out))
delta_twist = np.mat(np.zeros((6, 1)))
pos_delta = pos - pos_fk
delta_twist[:3, 0] = pos_delta
rot_delta = np.mat(np.eye(4))
rot_delta[:3, :3] = rot * rot_fk.T
rot_delta_angles = np.mat(trans.euler_from_matrix(rot_delta)).T
delta_twist[3:6, 0] = rot_delta_angles
J = self.jacobian(q_out)
J[3:6, :] *= np.sqrt(rot_weight)
delta_twist[3:6, 0] *= np.sqrt(rot_weight)
J_tinv = np.linalg.inv(J.T * J + np.diag(q_bias_weights) * np.eye(len(q_init))) * J.T
q_bias_diff = q_bias - q_out
q_bias_diff_normed = q_bias_diff * bias_vel / np.linalg.norm(q_bias_diff)
delta_q = q_bias_diff_normed + J_tinv * (delta_twist - J * q_bias_diff_normed)
q_out += delta_q
q_out = np.mat(self.clip_joints_safe(q_out.T.A[0])).T
return q_out