def TakeClosePic(self): # x – normalized, unitless, velocity along X-axis. # +1 and -1 correspond to the maximum velocity in the forward and backward directions, respectively. x = 1.0 y = 0.0 theta = 0.0 # index of image image_i = 0 frequency = 0.1 # get current time t0 = time.time() initRobotPosition = almath.Pose2D( self.motion_service.getRobotPosition(False)) # since now the robot is 1.5 away from the human # let the robot move 0.7 meters forward while taking photos(now the velocity is 0.1 meter per second) # in order to keep 0.8 meter distance away from human while ((time.time() - t0) <= 7.0): # once 7s has been passed, break the loop if (time.time() - t0) > 7.0: break # print "time difference :" + str((time.time() - t0)) self.motion_service.moveToward(x, y, theta, [["MaxVelXY", frequency]]) image_i += 1 self.GetImage(image_i) # stop the robot self.motion_service.stopMove() endRobotPosition = almath.Pose2D( self.motion_service.getRobotPosition(False)) robotMove = almath.pose2DInverse(initRobotPosition) * endRobotPosition robotMove.theta = almath.modulo2PI(robotMove.theta) print "Distance change and angle change after approaching the human:", robotMove
def moverPernas(xMetros, yMetros, eixoGraus, posturaStandInit = True): try: if posturaStandInit: Motor.posturaStandInit() motion = Motor.movimento() initPosition = almath.Pose2D(motion.getRobotPosition(True)) targetDistance = almath.Pose2D(xMetros, yMetros, eixoGraus * almath.PI / 180) expectedEndPosition = initPosition * targetDistance motion.setMoveArmsEnabled(True, True) varmovimento = motion.moveTo(xMetros, yMetros, eixoGraus * almath.PI / 180) positionErrorThresholdPos = 0.01 positionErrorThresholdAng = 0.03 # The move is finished so output realEndPosition = almath.Pose2D(motion.getRobotPosition(False)) positionError = realEndPosition.diff(expectedEndPosition) positionError.theta = almath.modulo2PI(positionError.theta) if (abs(positionError.x) < positionErrorThresholdPos and abs(positionError.y) < positionErrorThresholdPos and abs(positionError.theta) < positionErrorThresholdAng): print("Movimento OK") else: print("Movimento ERROR", positionError.toVector()) Motor.posturaCrouch() ModoAutonomo.on() time.sleep(0.25) ModoAutonomo.off() time.sleep(0.25) Motor.posturaStandInit() return varmovimento except Exception as e: print("Exception -> Motor.moverPernas():", e) return False
def return_home_with_alnavigation(self): cpt = 0 if self.slam_pos_in_world: for i in range(0, 100): robotPose = almath.Pose2D( self.services.ALNavigation.getRobotPositionInMap()[0]) center = almath.Pose2D(0.0, 0.0, 0.0) if robotPose.distance(center) > 0.3: self.navigation_future = self.services.ALNavigation.navigateToInMap( [0.0, 0.0], _async=True) yield self.navigation_future if robotPose.distance(center) < 0.3: poseDiff = robotPose.diff(center) if math.fabs(poseDiff.theta) > 0.2: self.navigation_future = self.services.ALMotion.moveTo( 0, 0, almath.modulo2PI( poseDiff.theta), _async=True) yield self.navigation_future else: cpt += 1 if cpt > 3: self.slam_pos_in_world = [0, 0] yield stk.coroutines.Return(True) return self.logger.info("End back home.") yield stk.coroutines.Return(False)
def callback(self, poseStamped): pose = Pose(poseStamped.pose.position, poseStamped.pose.orientation) quat = almath.Quaternion(pose.orientation.w, pose.orientation.x, pose.orientation.y, pose.orientation.z) rotation = almath.rotation3DFromQuaternion(quat) position3d = almath.Position3D(pose.position.x, pose.position.y, pose.position.z) worldToTarget = almath.Pose2D(position3d.x, position3d.y, rotation.wz) worldToRobot = almath.Pose2D(self.motionProxy.getRobotPosition(True)) robotToTarget = almath.pinv(worldToRobot) * worldToTarget robotToTarget.theta = almath.modulo2PI(robotToTarget.theta) self.motionProxy.moveTo(robotToTarget.x, robotToTarget.y, robotToTarget.theta)
def main(session): """ Move To: Small example to make Nao Move To an Objective. """ # Get the services ALMotion & ALRobotPosture. motion_service = session.service("ALMotion") posture_service = session.service("ALRobotPosture") # Wake up robot motion_service.wakeUp() # Send robot to Stand Init posture_service.goToPosture("StandInit", 0.5) ##################### ## Enable arms control by move algorithm ##################### motion_service.setMoveArmsEnabled(True, True) #~ motion_service.setMoveArmsEnabled(False, False) ##################### ## FOOT CONTACT PROTECTION ##################### #~ motion_service.setMotionConfig([["ENABLE_FOOT_CONTACT_PROTECTION",False]]) motion_service.setMotionConfig([["ENABLE_FOOT_CONTACT_PROTECTION", True]]) ##################### ## get robot position before move ##################### initRobotPosition = almath.Pose2D(motion_service.getRobotPosition(False)) X = 0.3 Y = 0.1 Theta = math.pi/2.0 motion_service.moveTo(X, Y, Theta, _async=True) # wait is useful because with _async moveTo is not blocking function motion_service.waitUntilMoveIsFinished() ##################### ## get robot position after move ##################### endRobotPosition = almath.Pose2D(motion_service.getRobotPosition(False)) ##################### ## compute and print the robot motion ##################### robotMove = almath.pose2DInverse(initRobotPosition)*endRobotPosition # return an angle between ]-PI, PI] robotMove.theta = almath.modulo2PI(robotMove.theta) print "Robot Move:", robotMove # Go to rest position motion_service.rest()
def main(robotIP, PORT=9559): motionProxy = ALProxy("ALMotion", robotIP, PORT) postureProxy = ALProxy("ALRobotPosture", robotIP, PORT) # Wake up robot motionProxy.wakeUp() # Send robot to Stand Init postureProxy.goToPosture("StandInit", 0.5) ##################### ## Enable arms control by move algorithm ##################### motionProxy.setMoveArmsEnabled(True, True) #~ motionProxy.setMoveArmsEnabled(False, False) ##################### ## FOOT CONTACT PROTECTION ##################### #~ motionProxy.setMotionConfig([["ENABLE_FOOT_CONTACT_PROTECTION",False]]) motionProxy.setMotionConfig([["ENABLE_FOOT_CONTACT_PROTECTION", True]]) ##################### ## get robot position before move ##################### initRobotPosition = m.Pose2D(motionProxy.getRobotPosition(False)) X = 0.0 Y = 0.0 Theta = math.pi/2.0 motionProxy.post.moveTo(X, Y, Theta) motionProxy.setAngles("HeadYaw", 0.6, 0.6) # wait is useful because with post moveTo is not blocking function motionProxy.waitUntilMoveIsFinished() ##################### ## get robot position after move ##################### endRobotPosition = m.Pose2D(motionProxy.getRobotPosition(False)) ##################### ## compute and print the robot motion ##################### robotMove = m.pose2DInverse(initRobotPosition)*endRobotPosition # return an angle between ]-PI, PI] robotMove.theta = m.modulo2PI(robotMove.theta) print "Robot Move:", robotMove # Go to rest position motionProxy.rest()
def main(robotIP, PORT=9559): motionProxy = ALProxy("ALMotion", robotIP, PORT) postureProxy = ALProxy("ALRobotPosture", robotIP, PORT) # Wake up robot motionProxy.wakeUp() # Send robot to Stand Init postureProxy.goToPosture("StandInit", 0.5) ##################### ## Enable arms control by move algorithm ##################### motionProxy.setMoveArmsEnabled(True, True) #~ motionProxy.setMoveArmsEnabled(False, False) ##################### ## FOOT CONTACT PROTECTION ##################### #~ motionProxy.setMotionConfig([["ENABLE_FOOT_CONTACT_PROTECTION",False]]) motionProxy.setMotionConfig([["ENABLE_FOOT_CONTACT_PROTECTION", True]]) ##################### ## get robot position before move ##################### initRobotPosition = m.Pose2D(motionProxy.getRobotPosition(False)) X = 0.3 Y = 0.1 Theta = math.pi / 2.0 motionProxy.post.moveTo(X, Y, Theta) # wait is useful because with post moveTo is not blocking function motionProxy.waitUntilMoveIsFinished() ##################### ## get robot position after move ##################### endRobotPosition = m.Pose2D(motionProxy.getRobotPosition(False)) ##################### ## compute and print the robot motion ##################### robotMove = m.pose2DInverse(initRobotPosition) * endRobotPosition # return an angle between ]-PI, PI] robotMove.theta = m.modulo2PI(robotMove.theta) print "Robot Move:", robotMove # Go to rest position motionProxy.rest()
def last_move(self, x, y, theta): value = yield self.services.ALMotion.moveTo(x, y, 0, MOVE_CONFIG_LOW_SPEED, _async=True) if value: value = yield self.services.ALMotion.moveTo( 0, 0, almath.modulo2PI(theta), _async=True) if value: self.pod_pos_in_world = [0.0, 0.0] self.pod_position_from_robot = None yield stk.coroutines.Return(True) yield stk.coroutines.Return(False)
def moveFunction(self, target): diff = self.getPositionDiff(target) print diff while (abs(diff.y) > 0.1): diff = self.getPositionDiff(target) print "turn:", diff resTurn = self.motion.moveTo(0, 0, almath.modulo2PI(diff.getAngle())) time.sleep(0.01) diff = self.getPositionDiff(target) diff = self.getPositionDiff(target) while (diff.x > 0.1): diff = self.getPositionDiff(target) print "move: ", diff fut = self.motion.moveTo(diff.x, diff.y, almath.modulo2PI(diff.getAngle()), _async=True) time.sleep(0.2) diff = self.getPositionDiff(target) try: fut.wait() except Exception, e: pass
def walking(motionProxy, X, Y, Theta): # Enable arms control by move algorithm motionProxy.setMoveArmsEnabled(True, True) # FOOT CONTACT PROTECTION motionProxy.setMotionConfig([["ENABLE_FOOT_CONTACT_PROTECTION", True]]) # get robot position before move initRobotPosition = almath.Pose2D(motionProxy.getRobotPosition(False)) motionProxy.post.moveTo(X, Y, Theta) # wait is useful because with post moveTo is not blocking function motionProxy.waitUntilMoveIsFinished() # get robot position after move endRobotPosition = almath.Pose2D(motionProxy.getRobotPosition(False)) # compute and print the robot motion robotMove = almath.pose2DInverse(initRobotPosition)*endRobotPosition # return an angle between [-PI, PI] robotMove.theta = almath.modulo2PI(robotMove.theta) print "Robot Move:", robotMove
def walking(motionProxy, X, Y, Theta): # Enable arms control by move algorithm motionProxy.setMoveArmsEnabled(True, True) # FOOT CONTACT PROTECTION motionProxy.setMotionConfig([["ENABLE_FOOT_CONTACT_PROTECTION", True]]) # get robot position before move initRobotPosition = almath.Pose2D(motionProxy.getRobotPosition(False)) motionProxy.post.moveTo(X, Y, Theta) # wait is useful because with post moveTo is not blocking function motionProxy.waitUntilMoveIsFinished() # get robot position after move endRobotPosition = almath.Pose2D(motionProxy.getRobotPosition(False)) # compute and print the robot motion robotMove = almath.pose2DInverse(initRobotPosition) * endRobotPosition # return an angle between [-PI, PI] robotMove.theta = almath.modulo2PI(robotMove.theta) print "Robot Move:", robotMove
def EjecuteMovimiento(x, y, Theta): x = 0.5 y = 0.0 initPosition = almath.Pose2D(motionProxy.getRobotPosition(True)) print "EJECUTE en x: " + str(x) + "\nen y: " + str( y) + "\nen Theta: " + str(Theta) targetDistance = almath.Pose2D(x, y, Theta * almath.PI / 180) expectedEndPosition = initPosition * targetDistance # enableArms = self.getParameter("Arms movement enabled") # self.motion.setMoveArmsEnabled(enableArms, enableArms) motionProxy.moveTo(x, y, Theta * almath.PI / 180) # The move is finished so output realEndPosition = almath.Pose2D(motionProxy.getRobotPosition(False)) positionError = realEndPosition.diff(expectedEndPosition) positionError.theta = almath.modulo2PI(positionError.theta) if (abs(positionError.x) < positionErrorThresholdPos and abs(positionError.y) < positionErrorThresholdPos and abs(positionError.theta) < positionErrorThresholdAng): #onArrivedAtDestination() print "on arrived destination" else: #onStoppedBeforeArriving(positionError.toVector()) print "onStoppedBeforeArriving" + str(positionError.toVector())
def return_home(self, research_360_activated=False): on_home = False id_aruco = None if self.need_approach_home() and not research_360_activated: self.aruco_future = self.approach_home() yield self.aruco_future if not self.aruco_future.value(): if self.services.DXHomeFinder: self.services.DXHomeFinder.reason_cant_go_home.setValue( "obstacle") yield stk.coroutines.Return(False) self.aruco_future = self.find_home(research_360_activated) coord_home = yield self.aruco_future if coord_home: id_aruco = coord_home[1] coord_home = coord_home[0] r, r_theta = self.get_polar_coord(coord_home) if id_aruco == 128 or (id_aruco == 448 and r > 0.5): id_aruco = 128 self.aruco_future = self.services.ALMotion.moveTo( 0, 0, almath.modulo2PI(r_theta), MOVE_CONFIG_LOW_SPEED, _async=True) yield self.aruco_future if self.aruco_future.value(): self.aruco_future = self.services.ALMotion.moveTo( r, 0, 0, MOVE_CONFIG_LOW_SPEED, _async=True) yield self.aruco_future if self.aruco_future.value(): on_home = True self.aruco_future = self.services.ALMotion.moveTo( 0, 0, almath.modulo2PI(coord_home[-1] - r_theta), MOVE_CONFIG_LOW_SPEED, _async=True) yield self.aruco_future else: on_home = True else: # can't find aruco if self.services.DXHomeFinder: self.services.DXHomeFinder.reason_cant_go_home.setValue( "no_target") if on_home: if id_aruco == 128: self.aruco_future = self.find_home() coord_home = yield self.aruco_future if coord_home: id_aruco = coord_home[1] coord_home = coord_home[0] if coord_home and id_aruco == 448: x, y, theta = coord_home self.aruco_future = self.last_move(x, y, theta) yield self.aruco_future if not self.aruco_future.value(): if self.services.DXHomeFinder: self.services.DXHomeFinder.reason_cant_go_home.setValue( "obstacle") yield stk.coroutines.Return(self.aruco_future.value()) else: # can't find aruco if self.services.DXHomeFinder: self.services.DXHomeFinder.reason_cant_go_home.setValue( "no_target") if self.services.DXHomeFinder: if self.services.DXHomeFinder.reason_cant_go_home.value( ) == "unknown": self.services.DXHomeFinder.reason_cant_go_home.setValue( "obstacle") yield stk.coroutines.Return(False)
def is_detected(self, value): if 1 <= value[6] <= 6: print "detection" print "Landmark " + str(value[6]) + " is detected!" self.memory_service.insertData("priority", 5) # Calculate the relative pose to the robot frame current_global_goal = self.memory_service.getData("goal") #print "current_global_goal: ",current_global_goal landmark_global_pose = self.landmark_pose.get_pose(value[6]) #print "landmark_global_pose: ",landmark_global_pose currVel = self.motion_service.getRobotVelocity() #print "currVel",currVel #print "current_global_goal",current_global_goal current_global_goal_2 = [ a - b * 1.2 for a, b in zip(current_global_goal, currVel) ] current_global_goal = current_global_goal_2 name = 'CameraBottom' frame = 2 use_sensor_values = True camera_pose = self.motion_service.getPosition( name, frame, use_sensor_values) temp = math.sqrt( math.pow(value[1] * 0.001, 2) + math.pow(value[2] * 0.001, 2)) x_offset = math.sqrt( math.pow(temp, 2) - math.pow(camera_pose[2], 2)) + camera_pose[0] y_offset = -value[0] * 0.001 theta_offset = -value[5] rotation_matrix = self.operation.rotation_matrix( (landmark_global_pose[2] - theta_offset)) transform = np.dot( rotation_matrix, np.array([[x_offset], [y_offset], [theta_offset]])) current_robot_pose = [ landmark_global_pose[0] - transform[0][0], landmark_global_pose[1] - transform[1][0], self.operation.normalize_angle(landmark_global_pose[2] - transform[2][0]) ] results = self.operation.global_to_local(current_global_goal[0], current_global_goal[1], current_global_goal[2], current_robot_pose[0], current_robot_pose[1], current_robot_pose[2]) # Adjust movement adjust_config = velocity_adjustment(results) init_pose = almath.Pose2D( self.motion_service.getRobotPosition(True)) target_distance = almath.Pose2D(results[0], results[1], results[2]) expected_end_pose = init_pose * target_distance self.motion_service.moveTo(float(results[0]), float(results[1]), float(results[2]), adjust_config) # Check if reach the goal real_end_pose = almath.Pose2D( self.motion_service.getRobotPosition(True)) position_error = real_end_pose.diff(expected_end_pose) position_error.theta = almath.modulo2PI(position_error.theta) if abs(position_error.x) < linear_error \ and abs(position_error.y) < linear_error \ and abs(position_error.theta) < angular_error: print "Arrived" self.memory_service.insertData("state", "finished") else: print "Not arrived"
def return_home(self, *args, **kwargs): on_home = False self.navigation_future = self.find_home() coord_home = yield self.navigation_future if coord_home: r, r_theta = self.get_polar_coord(coord_home) if r > 0.3: self.services.ALMotion.angleInterpolation(["HeadYaw", "HeadPitch"], [0, -0.26], 2, 1, _async=True) self.navigation_future = self.services.ALMotion.moveTo( 0, 0, almath.modulo2PI( r_theta), _async=True) yield self.navigation_future if self.navigation_future.value(): move_object = move.Move(self.session, self.logger, r, 1.5) self.navigation_future = move_object.run() yield self.navigation_future if self.navigation_future.value(): on_home = True robot_pose = almath.Pose2D(coord_home) center = almath.Pose2D(0.0, 0.0, 0.0) poseDiff = robot_pose.diff(center) self.aruco_future = self.services.ALMotion.moveTo( 0, 0, almath.modulo2PI( poseDiff.theta - r_theta), _async=True) yield self.aruco_future else: on_home = True else: # can't find slam position if self.services.DXHomeFinder: self.services.DXHomeFinder.reason_cant_go_home.setValue("no_target") if on_home: self.navigation_future = self.find_home() coord_home = yield self.navigation_future if coord_home: robot_pose = almath.Pose2D(coord_home) center = almath.Pose2D(0.0, 0.0, 0.0) poseDiff = robot_pose.diff(center) self.navigation_future = self.last_move( poseDiff.x, poseDiff.y, poseDiff.theta) yield self.navigation_future if not self.navigation_future.value(): if self.services.DXHomeFinder: self.services.DXHomeFinder.reason_cant_go_home.setValue("obstacle") yield stk.coroutines.Return(self.navigation_future.value()) else: # can't find slam position if self.services.DXHomeFinder: self.services.DXHomeFinder.reason_cant_go_home.setValue("no_target") if self.services.DXHomeFinder: if self.services.DXHomeFinder.reason_cant_go_home.value() == "unknown": self.services.DXHomeFinder.reason_cant_go_home.setValue("obstacle") yield stk.coroutines.Return(False)
def return_home(self, research_360_activated=False): on_home = False if self.need_approach_home() and not research_360_activated: self.pod_future = self.approach_home() yield self.pod_future if not self.pod_future.value(): if self.services.DXHomeFinder: self.services.DXHomeFinder.reason_cant_go_home.setValue( "obstacle") yield stk.coroutines.Return(False) self.pod_future = self.find_home(research_360_activated) coord_home = yield self.pod_future if coord_home: r, r_theta = self.get_polar_coord(coord_home) if r > 0.5: x, y, theta = coord_home x += 0.3 * math.cos(theta) y += 0.3 * math.sin(theta) r, r_theta = self.get_polar_coord([x, y, theta]) self.pod_future = self.services.ALMotion.moveTo( 0, 0, almath.modulo2PI(r_theta), MOVE_CONFIG_LOW_SPEED, _async=True) yield self.pod_future if self.pod_future.value(): self.pod_future = self.services.ALMotion.moveTo( r, 0, 0, MOVE_CONFIG_LOW_SPEED, _async=True) yield self.pod_future if self.pod_future.value(): on_home = True self.pod_future = self.services.ALMotion.moveTo( 0, 0, almath.modulo2PI(coord_home[-1] - r_theta + math.pi), MOVE_CONFIG_LOW_SPEED, _async=True) yield self.pod_future coord_home = None else: on_home = True else: # can't find pod if self.services.DXHomeFinder: self.services.DXHomeFinder.reason_cant_go_home.setValue( "no_target") if on_home: if not coord_home: self.pod_future = self.find_home() coord_home = yield self.pod_future if coord_home: x, y, theta = coord_home self.pod_future = self.last_move(x, y, theta) yield self.pod_future if not self.pod_future.value(): if self.services.DXHomeFinder: self.services.DXHomeFinder.reason_cant_go_home.setValue( "obstacle") yield stk.coroutines.Return(self.pod_future.value()) else: # can't find pod if self.services.DXHomeFinder: self.services.DXHomeFinder.reason_cant_go_home.setValue( "no_target") if self.services.DXHomeFinder: if self.services.DXHomeFinder.reason_cant_go_home.value( ) == "unknown": self.services.DXHomeFinder.reason_cant_go_home.setValue( "obstacle") yield stk.coroutines.Return(False)
def run(self): print "qaaaaaaaaaaa" while True: print "bbbbbbbbbbbbbbbbb" time.sleep(0.2) try: robotPose = self.navigation.getRobotPositionInMap() print self.currentTarget currentPose = almath.Pose2D(robotPose[0][0], robotPose[0][1], robotPose[0][2]) if (self.currentTarget != self.init): target = almath.Pose2D(currentPose.x, currentPose.y, self.angle) currentPoseToTarget = currentPose.inverse() * target thetaModulo2PI = almath.modulo2PI( currentPoseToTarget.theta) future = self.motion.moveTo(0, 0, thetaModulo2PI, _async=True) self.motion.angleInterpolationWithSpeed(["HeadPitch"], [0], 0.2, _async=True) future.wait() else: target = almath.Pose2D(currentPose.x, currentPose.y, self.initAngle) currentPoseToTarget = currentPose.inverse() * target thetaModulo2PI = almath.modulo2PI( currentPoseToTarget.theta) future = self.motion.moveTo(0, 0, thetaModulo2PI, _async=True) self.motion.angleInterpolationWithSpeed(["HeadPitch"], [0], 0.2, _async=True) future.wait() if self.currentTarget == self.firstTarget: #self.anime.say("This is the Item numero one, I can tell you a lot of informations about it!") self.anime.say("one ") time.sleep(self.tsec) self.currentTarget = self.secondTarget elif self.currentTarget == self.init: # self.anime.say("Do you want to know have more informations about our items?") self.anime.say("init") time.sleep(self.tsec) self.currentTarget = self.firstTarget elif self.currentTarget == self.secondTarget: #self.anime.say("This is the item numero two, it is really interesting for a lot of reasons!") self.anime.say("two") time.sleep(self.tsec) self.currentTarget = self.thirdTarget elif self.currentTarget == self.thirdTarget: #self.anime.say("This is the item numero three, what an interesting item!") self.anime.say("three") time.sleep(self.tsec) self.currentTarget = self.init self.moveFunction(self.currentTarget) except Exception, e: print "Error :" print str(e) raise except (KeyboardInterrupt, SystemExit): print '\nkeyboardinterrupt found' break