Esempio n. 1
0
def callBack(msg):
    pose_vel = PoseVelocity()
    e = tft.euler_from_quaternion([
        msg.glovesData[0].wristIMU.x, msg.glovesData[0].wristIMU.y,
        msg.glovesData[0].wristIMU.z, msg.glovesData[0].wristIMU.w
    ])
    print(e)
    x = e[0]
    y = e[1]
    z = e[2]
    # 旋转 just for left hand, horizon is zero;
    if y > 1.2 or y < -0.5:
        if y > 1.2:  #right
            pose_vel.twist_angular_z = -0.1
        elif y < -0.5:  #left
            pose_vel.twist_angular_z = 0.1
    elif z > 1 or z < -1:
        if z > 1:  # up
            pose_vel.twist_angular_y = -0.1
        elif z < -1:
            pose_vel.twist_angular_y = 0.1
    elif x > 1 or x < -1:
        if x > 1:  # ni
            pose_vel.twist_angular_z = -0.1
        elif x < -1:  # shun
            pose_vel.twist_angular_z = 0.1
    # 平移
    if msg.glovesData[0].fingersFlex[1].Joint1Stretch > 0.3:
        pose_vel.twist_linear_z = -0.1
    if msg.glovesData[0].fingersFlex[1].Joint1Stretch < -0.3:
        pose_vel.twist_linear_z = 0.1
    if msg.glovesData[0].fingersFlex[2].Joint1Stretch > 0.3:
        pose_vel.twist_linear_y = -0.1
    if msg.glovesData[0].fingersFlex[2].Joint1Stretch < -0.3:
        pose_vel.twist_linear_y = 0.1
    if msg.glovesData[0].fingersFlex[3].Joint1Stretch > 0.3:
        pose_vel.twist_linear_x = -0.1
    if msg.glovesData[0].fingersFlex[3].Joint1Stretch < -0.3:
        pose_vel.twist_linear_x = 0.1
        # CURRENT_VELOCITY[3] = 1 * dr #x: end effector self rotate,positive is shun
        # CURRENT_VELOCITY[4] = 1 * dp #y: up and down rotate (positive is down)
        # CURRENT_VELOCITY[5] = 1 * dyaw  #z: left and right rotate,positive is left
    pub.publish(pose_vel)
Esempio n. 2
0
 def run(self):
     rate = rospy.Rate(100)
     while not rospy.is_shutdown():
         vel = PoseVelocity()
         vel.twist_linear_x = self.x
         vel.twist_linear_y = self.y
         vel.twist_linear_z = self.z
         vel.twist_angular_x = 0.0
         vel.twist_angular_y = 0.0
         vel.twist_angular_z = 0.0
         self.pub.publish(vel)
         rate.sleep()
Esempio n. 3
0
    def publishArmLinControl(self):
        pose_vel_msg = PoseVelocity()
        pose_vel_msg.twist_linear_x = self.joy.axes[
            1] * self.v_lin_gain  # Up/Down Axis stick left
        pose_vel_msg.twist_linear_y = self.joy.axes[
            0] * self.v_lin_gain  # Left/Right Axis stick left
        pose_vel_msg.twist_linear_z = self.joy.axes[
            3] * self.v_lin_gain  # Up/Down Axis stick right
        pose_vel_msg.twist_angular_x = 0
        pose_vel_msg.twist_angular_y = 0
        pose_vel_msg.twist_angular_z = 0

        self.pub_pose_vel_cmd.publish(pose_vel_msg)
Esempio n. 4
0
    def publishArmAngControl(self):
        pose_vel_msg = PoseVelocity()
        pose_vel_msg.twist_linear_x = 0
        pose_vel_msg.twist_linear_y = 0
        pose_vel_msg.twist_linear_z = 0

        pose_vel_msg.twist_angular_x = self.joy.axes[
            1] * self.w_ang_gain  # Up/Down Axis stick left

        if self.joy.buttons[1] == 0:  # if lip is not pressed
            pose_vel_msg.twist_angular_y = self.joy.axes[
                0] * self.w_ang_gain  # Left/Right Axis stick left
        elif self.joy.buttons[1] == 1:  # if lip is pressed
            pose_vel_msg.twist_angular_z = self.joy.axes[
                0] * -self.w_ang_gain  # Left/Right Axis stick right

        self.pub_pose_vel_cmd.publish(pose_vel_msg)
Esempio n. 5
0
    def publishArmLinControl(self):
        pose_vel_msg = PoseVelocity()
        pose_vel_msg.twist_linear_x = self.joy.axes[
            1] * self.v_lin_gain  # Left\Right

        if self.joy.buttons[1] == 0:  # if lip is not pressed
            pose_vel_msg.twist_linear_y = self.joy.axes[
                0] * self.v_lin_gain  # Forward\Backward
        elif self.joy.buttons[1] == 1:  # if lip is pressed
            pose_vel_msg.twist_linear_z = self.joy.axes[
                0] * self.v_lin_gain  # Up/Down Axis stick right

        pose_vel_msg.twist_angular_x = 0
        pose_vel_msg.twist_angular_y = 0
        pose_vel_msg.twist_angular_z = 0

        self.pub_pose_vel_cmd.publish(pose_vel_msg)
Esempio n. 6
0
def goup():
    global vel, CURR_POSE, point_reached
    desired_z_up = 0.3
    current_pose = CURR_POSE
    position_error = desired_z_up - current_pose.position.z
    K_p = 4
    thresh = 0.005
    if abs(position_error) > thresh:
        vel_z = K_p * position_error
    else:
        point_reached = True
        vel_z = 0.0
        print("point reached: go up")
    global robot_type
    if robot_type == 'kinova':
        from kinova_msgs.msg import PoseVelocity
        vel = PoseVelocity()
        vel.twist_linear_x = 0.0
        vel.twist_linear_y = 0.0
        vel.twist_linear_z = vel_z
def servo_to_point(desired_centroid, depth, enable_z):
    global vel, point_reached, CURR_FORCE, CURR_POSE, count_against_stuck
    max_count_against_stuck = 300
    if centroid:
        cx_b = centroid[0]; cy_b = centroid[1];
        current_centroid = np.array([cx_b, cy_b])

        vel_z = 0.0
        
        thresh = 10
        max_norm = 0.05
        e = current_centroid - desired_centroid
        error_norm =np.linalg.norm(e) 

        if error_norm > thresh and count_against_stuck < max_count_against_stuck:            
            interaction = get_interaction(current_centroid, im_size, depth)
            i_inv = np.linalg.pinv(interaction)

            gain_vs = 0.008
            velocity_cam = np.matmul(i_inv, e)
            # velocity_b = cam_to_base(velocity_cam)[:2]
            velocity_b = velocity_cam
            velocity_b[1] = -velocity_b[1]

            velocity_b = gain_vs*velocity_b
            norm = np.linalg.norm(velocity_b)
            
            if norm > max_norm:
                velocity_b = (velocity_b / norm) * max_norm

            if enable_z:
                # Contact Management
                #desired_force = -14.0
                desired_force = -5.0
                force_error = desired_force - CURR_FORCE
                #K_d = 0.008
                K_d = 0.0 #0.001
                   
                current_pose = CURR_POSE

                if tool == "straight":
                    desired_z = 0.19
                else:
                    desired_z = 0.15

                position_error = desired_z - current_pose.position.z 
                #print("current z position: ", current_pose.position.z)
                K_p = 5

                #vel_z = K_d * force_error
                vel_z = K_d * force_error + K_p * position_error
                #vel_z = K_p * position_error

                max_vel_z = 0.03                    
                #vel_z = np.clip(vel_z, -max_vel_z, max_vel_z)  
                #print(vel_z)   

            count_against_stuck += 1
            print("count: ", count_against_stuck)
               
        else:
            point_reached = True
            #print("Point Reached?", point_reached)
            velocity_b = np.array([0, 0])
            count_against_stuck = 0
    else:
        print("No Assigned Centroid")
        velocity_b = np.array([0, 0])

    global robot_type
    if robot_type == 'kinova':
        from kinova_msgs.msg import PoseVelocity
        vel = PoseVelocity()
        vel.twist_linear_x = velocity_b[0]
        vel.twist_linear_y = velocity_b[1]
        vel.twist_linear_z = vel_z
    #TODO: UR5 implementation

    print("vel: ", velocity_b)