当前位置: 首页>>代码示例>>Python>>正文


Python PoseConv.to_pos_rot方法代码示例

本文整理汇总了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
开发者ID:gt-ros-pkg,项目名称:hrl-pr2-behaviors,代码行数:9,代码来源:pr2_arm_cart_base.py

示例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
开发者ID:Sean3Don,项目名称:hrl-kdl,代码行数:31,代码来源:kdl_kinematics.py

示例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)
开发者ID:gt-ros-pkg,项目名称:hrl-assistive,代码行数:9,代码来源:ellipsoid_controller.py

示例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
开发者ID:Sean3Don,项目名称:hrl-kdl,代码行数:10,代码来源:kdl_kinematics.py

示例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)
开发者ID:gt-ros-pkg,项目名称:hrl-assistive,代码行数:13,代码来源:cartesian_manager.py

示例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])
开发者ID:gt-ros-pkg,项目名称:hrl-pr2-behaviors,代码行数:22,代码来源:pr2_arm_jt_task.py

示例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
开发者ID:gt-ros-pkg,项目名称:hrl-assistive,代码行数:22,代码来源:controller_base.py

示例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
开发者ID:gt-ros-pkg,项目名称:hrl-assistive,代码行数:14,代码来源:visualize_poses.py

示例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)]
开发者ID:gt-ros-pkg,项目名称:hrl-assistive,代码行数:42,代码来源:ellipsoid_space.py

示例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
开发者ID:gt-ros-pkg,项目名称:hrl-assistive,代码行数:15,代码来源:controller_base.py

示例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)
开发者ID:gt-ros-pkg,项目名称:hrl-assistive,代码行数:26,代码来源:cartesian_manager.py

示例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
开发者ID:gt-ros-pkg,项目名称:hrl-assistive,代码行数:50,代码来源:ellipsoid_controller.py

示例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)
开发者ID:gt-ros-pkg,项目名称:hrl-assistive,代码行数:23,代码来源:controller_base.py

示例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)
开发者ID:gt-ros-pkg,项目名称:hrl-assistive,代码行数:23,代码来源:cartesian_manager.py

示例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
开发者ID:BetaS,项目名称:baxter_grip_object,代码行数:24,代码来源:kdl_kinematics.py


注:本文中的hrl_geom.pose_converter.PoseConv.to_pos_rot方法示例由纯净天空整理自Github/MSDocs等开源代码及文档管理平台,相关代码片段筛选自各路编程大神贡献的开源项目,源码版权归原作者所有,传播和使用请参考对应项目的License;未经允许,请勿转载。