def commander(self, distance, speed): vel = PoseVelocity() beginTime = rospy.Time.now() endTime = rospy.Duration(distance / float(abs(speed))) + beginTime while rospy.Time.now() < beginTime: vel.twist_linear_x = speed self.pub.publish(vel)
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)
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()
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)
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)
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)
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)