def cartesian_vel_to_joint_vel(vel, angular_speed, current_configuration, side, horizontal, endlink_angle, endeffector_offset): cur_pos, cur_ang, ret_endlink_angle = np.array( forward_kinematics(current_configuration, side, horizontal, return_endlink_angle=True, endeffector_offset=endeffector_offset)) cur_point = Point(*cur_pos) cur_target_joints = call_ik_solver(cur_point, side, horizontal, endlink_angle, endeffector_offset) if cur_target_joints is None: return np.array([0.] * len(joint_names)) cur_target_joints[4] -= cur_ang cart_speed = np.linalg.norm(vel) if cart_speed > MAX_DIST_LIN_SPEEDS: scale = MAX_DIST_LIN_SPEEDS / np.linalg.norm(vel) else: scale = 1 target_vector = cur_pos + scale * vel goal = Point(*list(target_vector)) target_joints = call_ik_solver(goal, side, horizontal, endlink_angle, endeffector_offset) if target_joints is not None: target_joints[4] = limit_joint_5(target_joints[4] - cur_ang) joint_velocities = (target_joints - np.array(current_configuration)) / scale joint_velocities[4] -= angular_speed # print joint_velocities[4] return joint_velocities else: return (cur_target_joints - np.array(current_configuration)) / scale
def cartesian_vel_to_joint_vel(vel, angular_speed, current_configuration, side, horizontal, endlink_angle, endeffector_offset): cur_pos, cur_ang, ret_endlink_angle = np.array(forward_kinematics(current_configuration, side, horizontal, return_endlink_angle=True, endeffector_offset=endeffector_offset)) cur_point = Point(*cur_pos) cur_target_joints = call_ik_solver(cur_point, side, horizontal, endlink_angle, endeffector_offset) if cur_target_joints is None: return np.array([0.] * len(joint_names)) cur_target_joints[4] -= cur_ang cart_speed = np.linalg.norm(vel) if cart_speed > MAX_DIST_LIN_SPEEDS: scale = MAX_DIST_LIN_SPEEDS / np.linalg.norm(vel) else: scale = 1 target_vector = cur_pos + scale * vel goal = Point(*list(target_vector)) target_joints = call_ik_solver(goal, side, horizontal, endlink_angle, endeffector_offset) if target_joints is not None: target_joints[4] = limit_joint_5(target_joints[4] - cur_ang) joint_velocities = (target_joints - np.array(current_configuration)) / scale joint_velocities[4] -= angular_speed # print joint_velocities[4] return joint_velocities else: return (cur_target_joints - np.array(current_configuration)) / scale
def execute_arm_ik_action(self, action_goal): assert isinstance(action_goal, MoveArmIKGoal) conf = call_ik_solver(action_goal.position, action_goal.side, action_goal.horizontal, action_goal.endlink_angle, action_goal.endeffector_offset) res = MoveArmIKResult() if conf is None: res.success = False res.reason = 'No IK solution found' return res conf[4] = get_forward_angle_joint_5(conf[4], action_goal.angle) action_result = MoveArmIKResult() if action_goal.velocity_controlled: if action_goal.max_speed is 0.0: speed = MAX_SPEED else: speed = action_goal.max_speed res.success = self.go_velocity_controlled([conf], speed, wait=False) client = self.arm_velocity_joint_client else: res.success = self.go_position_controlled([conf], wait=False) client = self.arm_position_joint_client rate = rospy.Rate(CHECK_STATUS_RATE) while client.get_state() is not GoalStatus.SUCCEEDED: if self.arm_ik_action_server.is_preempt_requested(): client.cancel_all_goals() action_result.success = False action_result.reason = 'Action Canceled' self.arm_ik_action_server.set_aborted(result=action_result) return if client.get_state() in [ GoalStatus.PREEMPTED, GoalStatus.ABORTED ]: client.cancel_all_goals() action_result.success = False action_result.reason = 'Action failed' self.arm_ik_action_server.set_preempted(result=action_result) return rate.sleep() action_result.success = True action_result.reason = 'Action succeeded' self.arm_ik_action_server.set_succeeded(result=action_result)
def move_arm_ik(self, req): assert isinstance(req, MoveArmIKRequest) conf = call_ik_solver(req.position, req.side, req.horizontal, req.endlink_angle,req.endeffector_offset) res = MoveArmIKResponse() if conf is None: res.success = False res.reason = 'No IK solution found' return res conf[4] = get_forward_angle_joint_5(conf[4], req.angle) if req.velocity_controlled: if req.max_speed == 0.0: speed = MAX_SPEED else: speed = req.max_speed res.success = self.go_velocity_controlled([conf], speed, wait=req.blocking) else: res.success = self.go_position_controlled([conf], wait=req.blocking) return res
def execute_linear_action(self, goal): assert isinstance(goal, MoveArmLinearGoal) over_effort_counter = 0 rate = rospy.Rate(50) result = MoveArmLinearResult() side = goal.side horizontal = goal.horizontal endlink_angle = goal.endlink_angle max_speed = goal.max_speed endeffector_offset = goal.endeffector_offset if max_speed < ARM_MOVING_THRESHOLD: result.success = False result.reason = 'Max speed is lower than arm moving threshold' self.linear_action_server.set_preempted(result) return if len(goal.points) < 2: result.success = False result.reason = 'Less than two points given' self.linear_action_server.set_preempted(result) return for target_pose in goal.points: conf = call_ik_solver(target_pose.position, goal.side, horizontal=goal.horizontal, endlink_angle=endlink_angle, endeffector_offset=endeffector_offset) if conf is None: result.success = False result.reason = 'No IK solution found' self.linear_action_server.set_preempted(result) return first_conf = call_ik_solver(goal.points[0].position, goal.side, horizontal=goal.horizontal, endlink_angle=endlink_angle, endeffector_offset=endeffector_offset) first_conf[4] = limit_joint_5(first_conf[4] - goal.points[0].angle) last_conf = call_ik_solver(goal.points[-1].position, goal.side, horizontal=goal.horizontal, endlink_angle=endlink_angle, endeffector_offset=endeffector_offset) last_conf[4] = limit_joint_5(last_conf[4] - goal.points[-1].angle) if not self.go_velocity_controlled( [first_conf], NORMAL_SPEED, action_server=self.linear_action_server): result.success = False result.reason = 'Stopped while going to first conf' self.linear_action_server.set_aborted(result) self.stop_arm() return for endeffector_pose in goal.points: target_pose = [ endeffector_pose.position.x, endeffector_pose.position.y, endeffector_pose.position.z ] target_ang = endeffector_pose.angle print target_pose while not rospy.is_shutdown(): if self.is_over_effort(): over_effort_counter += 1 rospy.logerr("%d, over effort", over_effort_counter) if over_effort_counter > LIMIT_OVER_EFFORT: result.success = False rospy.logerr("arm over-effort") result.reason = "arm over effort" self.arm_up_recover() self.linear_action_server.set_preempted(result) return if self.linear_action_server.is_preempt_requested(): self.stop_arm() result.success = False result.reason = 'Action aborted' self.linear_action_server.set_aborted(result) return cur_pose, cur_ang = forward_kinematics(self.configuration, side, horizontal, endlink_angle, endeffector_offset) if np.linalg.norm(np.array(cur_pose) - np.array(target_pose)) < THRESHOLD_LINEAR_MOVEMENT \ and abs(cur_ang - target_ang) < THRESHOLD_ANGULAR_MOVEMENT: break msg = self.get_linear_movement_msg( target_pose, target_ang, max_speed, side, horizontal=horizontal, endlink_angle=endlink_angle, endeffector_offset=endeffector_offset) self.velocity_pub.publish(msg) rate.sleep() if not self.go_velocity_controlled( [last_conf], NORMAL_SPEED, action_server=self.linear_action_server): result.success = False result.reason = 'Stopped while going to last conf' self.linear_action_server.set_aborted(result) self.stop_arm() return self.stop_arm() result.success = True result.reason = 'Movement Succeeded' self.linear_action_server.set_succeeded(result)
def get_sine(a, c, x): ret_y = a*math.sin(c*x) return x, ret_y a = 0.075 c = 12.5 * math.pi max_x = 0.155 height = -0.14 steps = 100 srv = rospy.ServiceProxy('/move_arm_linear', MoveArmLinear) req = MoveArmLinearRequest() req.horizontal = False req.side = 'left' req.max_speed = 0.03 step = 0 for i in range(steps+1): pos = EndeffectorPosition() x, y = get_sine(a, c, step) step += max_x/steps pos.position.x = origin[0] + y pos.position.y = origin[1] + max_x/2. - x - 0.025 pos.position.z = height print x, y, call_ik_solver(pos.position, req.side) req.points.append(pos) srv.call(req)
def execute_linear_action(self, goal): assert isinstance(goal, MoveArmLinearGoal) over_effort_counter = 0 rate = rospy.Rate(50) result = MoveArmLinearResult() side = goal.side horizontal = goal.horizontal endlink_angle = goal.endlink_angle max_speed = goal.max_speed endeffector_offset = goal.endeffector_offset if max_speed < ARM_MOVING_THRESHOLD: result.success = False result.reason = 'Max speed is lower than arm moving threshold' self.linear_action_server.set_preempted(result) return if len(goal.points) < 2: result.success = False result.reason = 'Less than two points given' self.linear_action_server.set_preempted(result) return for target_pose in goal.points: conf = call_ik_solver(target_pose.position, goal.side, horizontal=goal.horizontal, endlink_angle=endlink_angle, endeffector_offset=endeffector_offset) if conf is None: result.success = False result.reason = 'No IK solution found' self.linear_action_server.set_preempted(result) return first_conf = call_ik_solver(goal.points[0].position, goal.side, horizontal=goal.horizontal, endlink_angle=endlink_angle, endeffector_offset=endeffector_offset) first_conf[4] = limit_joint_5(first_conf[4] - goal.points[0].angle) last_conf = call_ik_solver(goal.points[-1].position, goal.side, horizontal=goal.horizontal, endlink_angle=endlink_angle, endeffector_offset=endeffector_offset) last_conf[4] = limit_joint_5(last_conf[4] - goal.points[-1].angle) if not self.go_velocity_controlled([first_conf], NORMAL_SPEED, action_server=self.linear_action_server): result.success = False result.reason = 'Stopped while going to first conf' self.linear_action_server.set_aborted(result) self.stop_arm() return for endeffector_pose in goal.points: target_pose = [endeffector_pose.position.x, endeffector_pose.position.y, endeffector_pose.position.z] target_ang = endeffector_pose.angle print target_pose while not rospy.is_shutdown(): if self.is_over_effort(): over_effort_counter += 1 rospy.logerr("%d, over effort", over_effort_counter) if over_effort_counter > LIMIT_OVER_EFFORT: result.success = False rospy.logerr("arm over-effort") result.reason = "arm over effort" self.arm_up_recover() self.linear_action_server.set_preempted(result) return if self.linear_action_server.is_preempt_requested(): self.stop_arm() result.success = False result.reason = 'Action aborted' self.linear_action_server.set_aborted(result) return cur_pose, cur_ang = forward_kinematics(self.configuration, side, horizontal, endlink_angle, endeffector_offset) if np.linalg.norm(np.array(cur_pose) - np.array(target_pose)) < THRESHOLD_LINEAR_MOVEMENT \ and abs(cur_ang - target_ang) < THRESHOLD_ANGULAR_MOVEMENT: break msg = self.get_linear_movement_msg(target_pose, target_ang, max_speed, side, horizontal=horizontal, endlink_angle=endlink_angle, endeffector_offset=endeffector_offset) self.velocity_pub.publish(msg) rate.sleep() if not self.go_velocity_controlled([last_conf], NORMAL_SPEED, action_server=self.linear_action_server): result.success = False result.reason = 'Stopped while going to last conf' self.linear_action_server.set_aborted(result) self.stop_arm() return self.stop_arm() result.success = True result.reason = 'Movement Succeeded' self.linear_action_server.set_succeeded(result)