コード例 #1
0
    def __init__(self, ):
        #python api
        self.api_psm3_arm = dvrk.arm('PSM3')
        self.dvrk_controller = dvrk_tf_module(userId="test",
                                              dst_path='./videos',
                                              rig_name="pu_dvrk_cam")

        #tf
        self.tf_world_psm3b = PyKDL.Frame()

        #subscribers
        self.psm1_suj_subs = rospy.Subscriber("/dvrk/PSM1/io/suj_clutch", Joy,
                                              self.setup_button_psm1_callback)

        #tf
        self.tf_world_psm3b_subs = rospy.Subscriber(
            "/pu_dvrk_tf/tf_world_psm3b", PoseStamped,
            self.tf_world_psm3b_callback)

        self.ar_orientation = PyKDL.Rotation.Quaternion(
            0.68175651, -0.27654879, 0.56096521, 0.37953506)
        self.dvrk_controller.ar_orientation = self.ar_orientation

        self.fix_orientation = PyKDL.Rotation.Quaternion(
            0.20611444, -0.10502740, 0.60974223, 0.75809003)
        self.base = Vector(*[0.23937060, -0.09208578, 0.05523316])
        self.position1 = Vector(*[0.27518547, -0.18335637, 0.06959790])
        self.position2 = Vector(*[0.23203641, -0.18565913, 0.06422155])
        self.position3 = Vector(*[0.26621665, -0.11688039, 0.07422218])
        self.position4 = Vector(*[0.21614252, -0.10163200, 0.07084345])
コード例 #2
0
    def makePoses(self):
        '''
            产生位姿矩阵序列
        '''

        poses = []
        points = self.makePoints()
        for i in range(len(points)):

            UX = Vector(points[i][0][0], points[i][0][1], points[i][0][2])
            UY = Vector(points[i][1][0], points[i][1][1], points[i][1][2])
            UZ = Vector(points[i][2][0], points[i][2][1], points[i][2][2])
            Rot = Rotation(UX, UY, UZ)
            pose = Pose()
            pose.position.x = points[i][3][0]
            pose.position.y = points[i][3][1]
            pose.position.z = points[i][3][2]

            q = Rot.GetQuaternion()
            pose.orientation.x = q[0]
            pose.orientation.y = q[1]
            pose.orientation.z = q[2]
            pose.orientation.w = q[3]

            poses.append(pose)

        return poses
コード例 #3
0
ファイル: spacenav_control.py プロジェクト: WPI-ME5205/AMBF
    def joy_cb(self, msg):

        self._lin_vel = Vector(-msg.axes[0], -msg.axes[1], msg.axes[2])
        self._ang_vel = Vector(-msg.axes[3], -msg.axes[4], msg.axes[5])

        self._active = True
        pass
コード例 #4
0
def rot_matrix_from_vecs(vec_a, vec_b):
    out = Rotation()
    vec_a.Normalize()
    vec_b.Normalize()
    vcross = vec_a * vec_b
    vdot = dot(vec_a, vec_b)
    # Check if the vectors are in the same direction
    if 1.0 - vdot < 0.1:
        return out
    # Or in the opposite direction
    elif 1.0 + vdot < 0.1:
        nx = Vector(1, 0, 0)
        temp_dot = dot(vec_a, nx)
        if -0.9 < abs(temp_dot) < 0.9:
            axis = vec_a * nx
            out = out.Rot(axis, 3.14)
        else:
            ny = Vector(0, 1, 0)
            axis = vec_a * ny
            out = out.Rot(axis, 3.14)
    else:
        skew_v = skew_mat(vcross)
        out = add_mat(
            add_mat(Rotation(), skew_v),
            scalar_mul(skew_v * skew_v, (1 - vdot) / (vcross.Norm()**2)))
    return out
コード例 #5
0
    def __init__(self, name, mtm_name):
        pose_str = name + 'pose'
        button_str = name + 'button'
        force_str = name + 'force_feedback'

        self._active = False
        self._scale = 0.0009
        self.pose = Frame(Rotation().RPY(0, 0, 0), Vector(0, 0, 0))
        self.base_frame = Frame(Rotation().RPY(0, 0, 0), Vector(0, 0, 0))
        self.tip_frame = Frame(Rotation().RPY(0, 0, 0), Vector(0, 0, 0))
        self.grey_button_pressed = False # Used as Position Engage Clutch
        self.white_button_pressed = False # Used as Gripper Open Close Binary Angle
        self._force = DeviceFeedback()
        self._force.force.x = 0
        self._force.force.y = 0
        self._force.force.z = 0
        self._force.position.x = 0
        self._force.position.y = 0
        self._force.position.z = 0

        self._pose_sub = rospy.Subscriber(pose_str, PoseStamped, self.pose_cb, queue_size=10)
        self._button_sub = rospy.Subscriber(button_str, DeviceButtonEvent, self.buttons_cb, queue_size=10)
        self._force_pub = rospy.Publisher(force_str, DeviceFeedback, queue_size=1)

        # External Clutch Buttons Requested by ICL
        extra_footPedal_str = '/footPedal'
        self._extra_pedal_cb = rospy.Subscriber(extra_footPedal_str, Vector3, self.extra_footpedal_cb, queue_size=1)
        self._engageMTM = True
        self._externalFootPedalMsg = Vector3()

        self._geomagicButtonMsg = DeviceButtonEvent()

        print('BINDING GEOMAGIC DEVICE: ', name, 'TO MOCK MTM DEVICE: ', mtm_name)
        self._mtm_handle = ProxyMTM(mtm_name)
        self._msg_counter = 0
コード例 #6
0
    def get_initial_orientation(self, robot):
        vec_pose_to_goal = Vector(
            self.position[0] - robot.get_position()[0],
            self.position[1] - robot.get_position()[1],
            0,
        )
        vec_pose_to_goal.Normalize()

        return np.pi + (np.arccos(vec_pose_to_goal[0]) if vec_pose_to_goal[1] >
                        0 else -np.arccos(vec_pose_to_goal[0]))
コード例 #7
0
    def runOneArm(self, arm):
        arm.obj_gui.App.update()
        # Move the Target Position Based on the GUI
        x = arm.obj_gui.x
        y = arm.obj_gui.y
        z = arm.obj_gui.z
        ro = arm.obj_gui.ro
        pi = arm.obj_gui.pi
        ya = arm.obj_gui.ya
        gr = arm.obj_gui.gr
        arm.target.set_pos(x, y, z)
        arm.target.set_rpy(ro, pi, ya)

        p = arm.base.get_pos()
        q = arm.base.get_rot()
        P_b_w = Vector(p.x, p.y, p.z)
        R_b_w = Rotation.Quaternion(q.x, q.y, q.z, q.w)
        T_b_w = Frame(R_b_w, P_b_w)
        p = arm.target.get_pos()
        q = arm.target.get_rot()
        P_t_w = Vector(p.x, p.y, p.z)
        R_t_w = Rotation.Quaternion(q.x, q.y, q.z, q.w)
        T_t_w = Frame(R_t_w, P_t_w)
        T_t_b = T_b_w.Inverse() * T_t_w
        # P_t_b = T_t_b.p
        # P_t_b_scaled = P_t_b / self.psm1_scale
        # T_t_b.p = P_t_b_scaled
        computed_q = compute_IK(T_t_b)

        # print('SETTING JOINTS: ')
        # print(computed_q)

        arm.base.set_joint_pos('baselink-yawlink', computed_q[0])
        arm.base.set_joint_pos('yawlink-pitchbacklink', computed_q[1])
        arm.base.set_joint_pos('pitchendlink-maininsertionlink', computed_q[2])
        arm.base.set_joint_pos('maininsertionlink-toolrolllink', computed_q[3])
        arm.base.set_joint_pos('toolrolllink-toolpitchlink', computed_q[4])
        arm.base.set_joint_pos('toolpitchlink-toolyawlink', -computed_q[5])
        arm.base.set_joint_pos('toolyawlink-toolgripper1link', gr)
        arm.base.set_joint_pos('toolyawlink-toolgripper2link', gr)

        for i in range(3):
            if arm.sensor.is_triggered(i) and gr <= 0.2:
                sensed_obj = arm.sensor.get_sensed_object(i)
                if sensed_obj == 'Needle':
                    if not arm.grasped[i]:
                        arm.actuators[i].actuate(sensed_obj)
                        arm.grasped[i] = True
                        print('Grasping Sensed Object Names', sensed_obj)
                else:
                    if gr > 0.2:
                        arm.actuators[i].deactuate()
                        arm.grasped[i] = False
コード例 #8
0
 def __init__(self):
     self.ambf_data = OrderedDict()
     self.ambf_data['name'] = ''
     self.ambf_data['parent'] = ''
     self.ambf_data['child'] = ''
     self.ambf_data['parent axis'] = {'x': 0, 'y': 0.0, 'z': 1.0}
     self.ambf_data['parent pivot'] = {'x': 0, 'y': 0.0, 'z': 0}
     self.ambf_data['child axis'] = {'x': 0, 'y': 0.0, 'z': 1.0}
     self.ambf_data['child pivot'] = {'x': 0, 'y': 0.0, 'z': 0}
     self.ambf_data['joint limits'] = {'low': -1.2, 'high': 1.2}
     self.ambf_data['controller'] = {'P': 1000, 'I': 0, 'D': 10}
     self.origin = Vector()
     self.axis = Vector(0.0, 0.0, 1.0)
コード例 #9
0
    def __init__(self, name):
        pose_topic_name = name + 'pose'
        twist_topic_name = name + 'twist'
        button_topic_name = name + 'button'
        force_topic_name = name + 'force_feedback'

        self._active = False
        self._scale = 0.0002
        self.pose = Frame(Rotation().RPY(0, 0, 0), Vector(0, 0, 0))
        self.twist = PyKDL.Twist()
        # This offset is to align the pitch with the view frame
        R_off = Rotation.RPY(0.0, 0, 0)
        self._T_baseoffset = Frame(R_off, Vector(0, 0, 0))
        self._T_baseoffset_inverse = self._T_baseoffset.Inverse()
        self._T_tipoffset = Frame(Rotation().RPY(0, 0, 0), Vector(0, 0, 0))
        self.clutch_button_pressed = False  # Used as Position Engage Clutch
        self.gripper_button_pressed = False  # Used as Gripper Open Close Binary Angle
        self._force = DeviceFeedback()
        self._force.force.x = 0
        self._force.force.y = 0
        self._force.force.z = 0
        self._force.position.x = 0
        self._force.position.y = 0
        self._force.position.z = 0

        self._pose_sub = rospy.Subscriber(pose_topic_name,
                                          PoseStamped,
                                          self.pose_cb,
                                          queue_size=1)
        self._twist_sub = rospy.Subscriber(twist_topic_name,
                                           Twist,
                                           self.twist_cb,
                                           queue_size=1)

        ###### Commented
        self._button_sub = rospy.Subscriber(button_topic_name,
                                            DeviceButtonEvent,
                                            self.buttons_cb,
                                            queue_size=1)
        self._force_pub = rospy.Publisher(force_topic_name,
                                          DeviceFeedback,
                                          queue_size=1)

        self.switch_psm = False

        self._button_msg_time = rospy.Time.now()
        self._switch_psm_duration = rospy.Duration(0.5)

        print('Creating Geomagic Device Named: ', name, ' From ROS Topics')
        self._msg_counter = 0
コード例 #10
0
    def move_cv(self, twist, dt):
        if type(twist) in [np.array, np.ndarray]:
            v = Vector(twist[0], twist[1], twist[2]) * dt
            w = Vector(twist[3], twist[4], twist[5]) * dt
        elif type(twist) is Twist:
            v = twist.vel * dt
            w = twist.rot * dt
        else:
            raise TypeError

        T_c_w = self.get_T_c_w()
        T_cmd = Frame(Rotation.RPY(w[0], w[1], w[2]), v)
        self.move_cp(T_c_w * T_cmd)
        pass
コード例 #11
0
def attach_needle(needle, link):
    error = 1000
    if link is None:
        print('Not a valid link, returning')
        return
    while error > 0.1 and not rospy.is_shutdown():
        P_tINw = Vector(link.get_pos().x, link.get_pos().y, link.get_pos().z)

        # R_tINw = Rotation.Quaternion(tool_yaw_link.get_rot().x,
        #                              tool_yaw_link.get_rot().y,
        #                              tool_yaw_link.get_rot().x,
        #                              tool_yaw_link.get_rot().w)

        R_tINw = Rotation.RPY(link.get_rpy()[0],
                              link.get_rpy()[1],
                              link.get_rpy()[2])

        # we need to move the needle based on the Pose of the toolyawlink.
        # The yawlink's negative Y direction faces the graspp
        # position. Therefore, lets add a small offset to the P of yawlink.
        y_offset = Vector(0, -0.09, 0)
        P_nINw = P_tINw + R_tINw * y_offset

        # If you want to rotate the needle to a certain relative orientation
        # add another Rotation and multiply on the R.H.S of R_tINw in the
        # Equation below.
        R_nINw = R_tINw * Rotation.RPY(0, 0, 3.14)

        needle.set_pos(P_nINw[0], P_nINw[1], P_nINw[2])
        needle.set_rot(R_nINw.GetQuaternion())
        time.sleep(0.001)
        P_tINw = Vector(link.get_pos().x, link.get_pos().y, link.get_pos().z)
        P_nINw = Vector(needle.get_pos().x,
                        needle.get_pos().y,
                        needle.get_pos().z)
        error = (P_tINw - P_nINw).Norm()
        print error

    # Wait for the needle to get there
    time.sleep(3)

    # You should see the needle in the center of the two fingers.
    # If the gripper is not already closed, you shall have to manually
    # close it to grasp the needle. You should probably automate this in the testIK script.

    # Don't forget to release the pose command from the needle. We can
    # do so by calling:
    needle.set_force(0, 0, 0)
    needle.set_torque(0, 0, 0)
コード例 #12
0
    def __init__(self, args):

        self.args = args
        self.psm3 = AutonomyModule(
            activate_ar=args.activate_ar,
            activate_debug_visuals=args.activate_debug_visuals)
        self.model, self.labels_dict = create_model()
        self.centroid_module = utils.centroid_module()

        #Sleep until the subscribers are ready.
        time.sleep(0.20)
        self.sleep_time = 1.5

        #Fix landmarks
        self.base_position = Vector(*[+0.04527744, -0.02624656, -0.02356771])
        self.height_low = +0.07217540
        self.height_medium = +0.06053191
        self.height_high = self.height_low - 0.05

        #Flags
        self.autonomy_on = False

        #############
        #Subscribers#
        #############
        self.autonomy_flag_subs = rospy.Subscriber(
            "/recording_module/autonomy_active", Bool, self.autonomy_callback)
コード例 #13
0
def pose_cb(data):
    global trans
    p = data.pose
    trans = Frame(
        Rotation.Quaternion(p.orientation.x, p.orientation.y, p.orientation.z,
                            p.orientation.w),
        Vector(p.position.x, p.position.y, p.position.z))
コード例 #14
0
    def compute_cam_transfrom_from_gesture(self):
        if self._footpedals.cam_btn_pressed:
            # Take the Norm length
            init_vec = Vector(self._init_vec[0], self._init_vec[1], self._init_vec[2])
            init_length = PyKDL.dot(init_vec, init_vec)
            mtmr_cur_pose = self._mtmr.get_adjusted_pose()
            mtml_cur_pose = self._mtml.get_adjusted_pose()

            r_cur_pos = mtmr_cur_pose.p
            l_cur_pos = mtml_cur_pose.p

            cur_vec = l_cur_pos - r_cur_pos
            cur_length = PyKDL.dot(cur_vec, cur_vec)

            delta_len = cur_length - init_length
            delta_rot = get_rot_mat_from_vecs(cur_vec, init_vec)

            r = round(180.0/1.57079 * delta_rot.GetRPY()[0], 2)
            p = round(180.0/1.57079 * delta_rot.GetRPY()[1], 2)
            y = round(180.0/1.57079 * delta_rot.GetRPY()[2], 2)

            # print 'ZOOM', round(delta_len, 3), 'RPY', r, p, y

        else:
            mtmr_init_pose = self._mtmr.get_adjusted_pose()
            mtml_init_pose = self._mtml.get_adjusted_pose()

            self._r_init_pos = mtmr_init_pose.p
            self._l_init_pos = mtml_init_pose.p
            self._init_vec = self._l_init_pos - self._r_init_pos
コード例 #15
0
def compute_lin_over_thresh(err, thres):
    error_sign = get_vec_sign(err)
    abs_error = get_abs_vec(err)
    net_thres = abs_error - thres
    over_thres = get_elementwise_max(net_thres, Vector(0, 0, 0))
    over_thres = element_wise_mul(error_sign, over_thres)
    return over_thres
コード例 #16
0
def main():
    psm3 = PSM3Arm()

    #Sleep until the subscribers are ready.
    time.sleep(0.20)

    sleep_time = 0.08
    # goals = [Vector(0,0,-0.02),Vector(0,0,-0.005), Vector(0.0424, 0.0106,-0.02), Vector(0.0212,0,-0.02)]

    #Corners
    cols, rows = 6, 6
    chessboard_scale = 1.6 / 100  #1.065 / 100
    objp = np.zeros((cols * rows, 3), np.float32)
    objp[:, :2] = np.mgrid[0:cols, 0:rows].T.reshape(-1, 2)
    low_chessboard = objp * chessboard_scale + np.array([0, 0, -0.0010])

    answer = raw_input(
        "have you check the matrices and the offset are up to date? if yes write 'Y' to continue, else don't move the robot."
    )
    if answer != 'Y':
        print("Exiting the program")
        exit(0)
    else:
        print("Start moving arm 5 times")
        for i in range(1):
            print("Movement {:d}".format(i))
            for i in range(low_chessboard.shape[0]):
                low_goal = Vector(*low_chessboard[i, :])
                high_goal = Vector(*(low_chessboard[i, :] +
                                     np.array([0, 0, -0.02])))

                print("goal {:d} ".format(i), low_goal)
                print("goal {:d} ".format(i), high_goal)
                print("\n")

                psm3.move_psm3_to(high_goal)
                time.sleep(sleep_time)
                psm3.move_psm3_to(low_goal)
                time.sleep(sleep_time)

    try:
        while not rospy.core.is_shutdown():
            rospy.rostime.wallsleep(0.25)

    except KeyboardInterrupt:
        print("Shutting down")
コード例 #17
0
 def update_arm_pose(self):
     gui = self.GUI
     gui.App.update()
     T_t_b = Frame(Rotation.RPY(gui.ro, gui.pi, gui.ya),
                   Vector(gui.x, gui.y, gui.z))
     self.arm.move_cp(T_t_b)
     self.arm.set_jaw_angle(gui.gr)
     self.arm.run_grasp_logic(gui.gr)
コード例 #18
0
def to_kdl_vec(urdf_vec):
    # Default this vector to x axis consistent with the URDF convention
    v = Vector(1.0, 0, 0)
    if urdf_vec is not None:
        xyz = [float(i) for i in urdf_vec.attrib['xyz'].split()]
        for i in range(0, 3):
            v[i] = xyz[i]
    return v
コード例 #19
0
    def build_chain(self):
        self.chain = Chain()
        for e in self._config:
            v = Vector(e["xyzrpy"][0], e["xyzrpy"][1], e["xyzrpy"][2])
            r = Rotation.RPY(e["xyzrpy"][3], e["xyzrpy"][4], e["xyzrpy"][5])
            j = Joint(e["name"], Joint.None)

            f = Frame(r, v)

            if e["type"] == "rotx":
                axis = Vector(1.0, 0.0, 0.0)
                j = Joint(e["name"], f.p, f.M * axis, Joint.RotAxis)
            elif e["type"] == "roty":
                axis = Vector(0.0, 1.0, 0.0)
                j = Joint(e["name"], f.p, f.M * axis, Joint.RotAxis)
            elif e["type"] == "rotz":
                axis = Vector(0.0, 0.0, 1.0)
                j = Joint(e["name"], f.p, f.M * axis, Joint.RotAxis)
            elif e["type"] == "transx":
                axis = Vector(1.0, 0.0, 0.0)
                j = Joint(e["name"], f.p, f.M * axis, Joint.TransAxis)
            elif e["type"] == "transy":
                axis = Vector(0.0, 1.0, 0.0)
                j = Joint(e["name"], f.p, f.M * axis, Joint.TransAxis)
            elif e["type"] == "transz":
                axis = Vector(0.0, 0.0, 1.0)
                j = Joint(e["name"], f.p, f.M * axis, Joint.TransAxis)

            self.chain.addSegment(
                Segment(e["name"] + "seg", j, f, RigidBodyInertia()))
        self.fksolverpos = ChainFkSolverPos_recursive(self.chain)
コード例 #20
0
    def __init__(self):
        rospy.init_node('dvrk_slicer')

        self._mtml = DvrkArm('MTML')
        self._mtmr = DvrkArm('MTMR')

        self._mtmr_pos_pre = None
        self._mtml_pos_pre = None

        self._footpedals = DvrkFootPedals()

        self._cam_transform = Frame()
        self._probe_transform = Frame()

        self._igtl_cam_trans = igtltransform()
        self._igtl_cam_trans.name = 'CameraTransform'
        self._igtl_cam_trans.transform.rotation.w = 1.0
        self._igtl_probe_trans = igtltransform()
        self._igtl_probe_trans.name = 'ProbeTransform'
        self._igtl_probe_trans.transform.rotation.w = 1.0
        self._igtl_fiducial_point = igtlpoint()
        self._igtl_fiducial_point.name = 'ENTRY'
        self._fiducial_offset = Frame(Rotation().RPY(0, 0, 0), Vector(0, 0, 0))
        self._igtl_text = igtlstring()

        self._igtl_cam_trans_pub = rospy.Publisher('/IGTL_TRANSFORM_OUT', igtltransform, queue_size=1)
        self._igtl_probe_trans_pub = rospy.Publisher('/IGTL_TRANSFORM_OUT', igtltransform, queue_size=1)
        self._igtl_fiducial_pub = rospy.Publisher('/IGTL_POINT_OUT', igtlpoint, queue_size=1)
        self._igtl_status_pub = rospy.Publisher('/IGTL_TEXT_OUT', igtlstring, queue_size=1)

        self._rate = rospy.Rate(120)

        self._pub_msg_pairs = dict()
        self._pub_msg_pairs[self._igtl_cam_trans_pub] = self._igtl_cam_trans
        self._pub_msg_pairs[self._igtl_probe_trans_pub] = self._igtl_probe_trans
        self._pub_msg_pairs[self._igtl_fiducial_pub] = self._igtl_fiducial_point
        # self._pub_msg_pairs[self._igtl_status_pub] = self._igtl_text

        self._fiducial_placement_modes = ['ENTRY', 'TARGET']
        self._fiducial_placement_active_mode = 0

        self._cam_init_pose_computed = False
        self._r_init_pos = Vector()
        self._l_init_pos = Vector()
        self._init_vec = Vector()
コード例 #21
0
 def _update_camera_pose(self):
     if self._pose_changed is True:
         p = self.camera_handle.get_pos()
         q = self.camera_handle.get_rot()
         P_c_w = Vector(p.x, p.y, p.z)
         R_c_w = Rotation.Quaternion(q.x, q.y, q.z, q.w)
         self._T_c_w = Frame(R_c_w, P_c_w)
         self._T_w_c = self._T_c_w.Inverse()
         self._pose_changed = False
コード例 #22
0
 def pose_cb(self, msg):
     self._pose = msg
     self._frame.p = Vector(self._pose.pose.position.x,
                            self._pose.pose.position.y,
                            self._pose.pose.position.z)
     self._frame.M = Rotation.Quaternion(self._pose.pose.orientation.x,
                                         self._pose.pose.orientation.y,
                                         self._pose.pose.orientation.z,
                                         self._pose.pose.orientation.w)
コード例 #23
0
 def _update_base_pose(self):
     if not self._base_pose_updated:
         p = self.base.get_pos()
         q = self.base.get_rot()
         P_b_w = Vector(p.x, p.y, p.z)
         R_b_w = Rotation.Quaternion(q.x, q.y, q.z, q.w)
         self._T_b_w = Frame(R_b_w, P_b_w)
         self._T_w_b = self._T_b_w.Inverse()
         self._base_pose_updated = True
コード例 #24
0
def convert_mat_to_frame(mat):
    frame = Frame(Rotation.RPY(0, 0, 0), Vector(0, 0, 0))
    for i in range(3):
        for j in range(3):
            frame[(i, j)] = mat[i, j]

    for i in range(3):
        frame.p[i] = mat[i, 3]

    return frame
コード例 #25
0
ファイル: psmIK.py プロジェクト: JackHaoyingZhou/ambf
def test_ik(x, y, z, rx, ry, rz):
    Rx = Rotation.RPY(rx, 0.0, 0.0)
    Ry = Rotation.RPY(0.0, ry, 0.0)
    Rz = Rotation.RPY(0.0, 0.0, rz)

    tip_offset_rot = Rotation.RPY(np.pi, 0, np.pi/2)
    req_rot = tip_offset_rot * Rz * Ry * Rx
    req_pos = Vector(x, y, z)
    T_7_0 = Frame(req_rot, req_pos)
    # print "REQ POSE \n", round_transform(convert_frame_to_mat(T_7_0), 3), "\n\n--------\n\n"
    compute_IK(T_7_0)
コード例 #26
0
    def __init__(self, leader, psm):
        self.counter = 0
        self.leader = leader
        self.psm = psm

        self.init_xyz = Vector(0.0, 0.0, -1.0)
        self.init_rpy = Rotation.RPY(3.14, 0.0, 1.57079)

        self.cmd_xyz = self.init_xyz
        self.cmd_rpy = self.init_rpy

        self.T_IK = None
コード例 #27
0
ファイル: spacenav_control.py プロジェクト: WPI-ME5205/AMBF
    def process_commands(self):
        if self._active and self._obj_handle is not None:
            if self._obj_handle.is_wd_expired():
                self._obj_active = False

            if not self._obj_active:
                cur_pos = Vector(self._obj_handle.get_pos().x,
                                 self._obj_handle.get_pos().y,
                                 self._obj_handle.get_pos().z)

                cur_rpy = Vector(self._obj_handle.get_rpy()[0],
                                 self._obj_handle.get_rpy()[1],
                                 self._obj_handle.get_rpy()[2])

                if self._obj_handle.is_wd_expired:
                    self._obj_active = True
            else:
                cur_pos = self._cmd_pos
                cur_rpy = self._cmd_rpy

            rot = Rotation.RPY(0, 0, 0)
            if self._ambf_cam_handle is not None:
                cam_rpy = self._ambf_cam_handle.get_rpy()
                rot = Rotation.RPY(cam_rpy[0], cam_rpy[1], cam_rpy[2])

            self._cmd_pos = cur_pos + rot * (self._lin_vel * self._lin_scale)
            self._cmd_rpy = cur_rpy + (self._ang_vel * self._ang_scale)

            self._obj_handle.set_pos(self._cmd_pos[0], self._cmd_pos[1],
                                     self._cmd_pos[2])

            self._obj_handle.set_rpy(self._cmd_rpy[0], self._cmd_rpy[1],
                                     self._cmd_rpy[2])

            if self._msg_counter % 500 == 0:
                pass
            if self._msg_counter >= 1000:
                self._msg_counter = 0

            self._msg_counter = self._msg_counter + 1
コード例 #28
0
ファイル: utils.py プロジェクト: kaixinguo360/BrickBuilder
def pose_to_frame(pose):
    # type: (object) -> Frame
    if isinstance(pose, PoseStamped):
        pose = pose.pose
    assert isinstance(pose, Pose)

    pos = Vector(pose.position.x, pose.position.y, pose.position.z)

    rot = Rotation.Quaternion(pose.orientation.x, pose.orientation.y,
                              pose.orientation.z, pose.orientation.w)

    frame = Frame(rot, pos)

    return frame
コード例 #29
0
    def __init__(self, arm_name):
        pose_str = '/dvrk/' + arm_name + '/position_cartesian_current'
        wrench_str = '/dvrk/' + arm_name + '/set_wrench_body'
        gripper_str = '/dvrk/' + arm_name + '/state_gripper_current'
        status_str = '/dvrk/' + arm_name + '/status'

        self._mtm_arm_type = None

        if arm_name == 'MTMR':
            self._mtm_arm_type = 0
        elif arm_name == 'MTML':
            self._mtm_arm_type = 1
        else:
            print('SPECIFIED ARM: ', arm_name)
            print('WARNING, MTM ARM TYPE NOT UNDERSTOOD, SHOULD BE MTMR or MTML')
        pass

        self.pose = Frame()
        self.pose.p = Vector(0, 0, 0)
        self.pose.M = Rotation.Quaternion(0, 0, 0, 1)
        self.buttons = 0

        self._commanded_force = Vector(0, 0, 0)

        self._gripper_min_angle = -3.16
        self._gripper_max_angle = 1.2
        self._gripper_angle = JointState()
        self._gripper_angle.position.append(0)

        self._pose_pub = rospy.Publisher(pose_str, PoseStamped, queue_size=1)
        self._gripper_pub = rospy.Publisher(gripper_str, JointState, queue_size=1)
        self._status_pub = rospy.Publisher(status_str, Empty, queue_size=1)

        self._force_sub = rospy.Subscriber(wrench_str, Wrench, self.force_cb, queue_size=10)

        self._foot_pedal = ProxyButtons()
        pass
コード例 #30
0
    def ar_animation(self, init_pt, end_pt, n=40, animation_sleep=0.040):
        timeout = 14
        init_time = time.time()

        trajectory = self.generate_trajectory(init_pt, end_pt, n=n)

        for i in range(n):

            new_position = Vector(*list(trajectory[:, i]))
            self.set_activate_ar(
                PyKDL.Frame(self.fix_orientation, new_position))
            time.sleep(animation_sleep)

            if time.time() - init_time > timeout:
                break