def __init__(self): self.n_history = 4 self.rotateUnit = 3.14/72 self.actionNum = 8 self.actions = np.array([ [self.rotateUnit, 0.0, 0.0, 0.0, 0.0, 0.0], [-self.rotateUnit, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, self.rotateUnit, 0.0, 0.0, 0.0, 0.0], [0.0, -self.rotateUnit, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, self.rotateUnit, 0.0, 0.0, 0.0], [0.0, 0.0, -self.rotateUnit, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, self.rotateUnit, 0.0, 0.0], [0.0, 0.0, 0.0, -self.rotateUnit, 0.0, 0.0], #[0.0, 0.0, 0.0, 0.0, self.rotateUnit, 0.0], [0.0, 0.0, 0.0, 0.0, -self.rotateUnit, 0.0], #[0.0, 0.0, 0.0, 0.0, 0.0, self.rotateUnit], [0.0, 0.0, 0.0, 0.0, 0.0, -self.rotateUnit] ]) self.robotJointAngles = moveToJointAnglesGoal() self.TCPForce = 0 self.done = False self.initJointAngles = moveToJointAnglesGoal() self.initJointAngles.a1 = -120.0 /180*math.pi self.initJointAngles.a2 = -90.0 /180*math.pi self.initJointAngles.a3 = 90.0 /180*math.pi self.initJointAngles.a4 = -90.0 /180*math.pi self.initJointAngles.a5 = 90.0 /180*math.pi self.initJointAngles.a6 = 0 /180*math.pi self.initPose = moveToCartesianPoseGoal() self.initPose.x = 0.4869 self.initPose.y = 0.1090 self.initPose.z = 0.5140 self.initPose.u = -90.0 /180*math.pi self.initPose.v = 0.0 /180*math.pi self.initPose.w = 180.0 /180*math.pi self.initArea = 0 self.area = 0 self.startTime = rospy.get_time() self.timeOver = False #FOR Camera self.bridge = CvBridge() self.image_sub = rospy.Subscriber("/gazebo/camera1/image_raw",Image,self.cameraCallback) self.image_raw = None self.image_resize = None rospy.loginfo("waiting for server ...") _jointAnglesSub = rospy.Subscriber('/ur_moveit_planner/currentJointAngles', JointQuantity, self.callbackJointAngles) self.action_client = actionlib.SimpleActionClient("ur_moveit_planner/moveToJointAnglesAction",moveToJointAnglesAction) self.action_client.wait_for_server() rospy.loginfo("server ON") self.spawn_gazebo_models() self.reset()
def move_random(self): r = random.randint(0, 6) action = self.actions[r] nextRobotPose = moveToCartesianPoseGoal() currentRobotPose = copy.deepcopy(self.robotPose) nextRobotPose.x = currentRobotPose.position.x + action[0] nextRobotPose.y = currentRobotPose.position.y + action[1] nextRobotPose.z = currentRobotPose.position.z + action[2] nextRobotPose.u = currentRobotPose.rpy.u nextRobotPose.v = currentRobotPose.rpy.v nextRobotPose.w = currentRobotPose.rpy.w return self.move(nextRobotPose)
def move_action(self, act): action = self.actions[act] #print act, action nextRobotPose = moveToCartesianPoseGoal() currentRobotPose = copy.deepcopy(self.robotPose) nextRobotPose.x = currentRobotPose.position.x + action[0] nextRobotPose.y = currentRobotPose.position.y + action[1] nextRobotPose.z = currentRobotPose.position.z + action[2] nextRobotPose.u = currentRobotPose.rpy.u nextRobotPose.v = currentRobotPose.rpy.v nextRobotPose.w = currentRobotPose.rpy.w return self.move(nextRobotPose)
def __init__(self): self.coordinateUnit = 0.01 self.rotateUnit = 1.0 """ self.actions = np.array([ [self.coordinateUnit, 0.0, 0.0, 0.0], [-self.coordinateUnit, 0.0, 0.0, 0.0], [0.0, self.coordinateUnit, 0.0, 0.0], [0.0, -self.coordinateUnit, 0.0, 0.0], [0.0, 0.0, self.coordinateUnit, 0.0], [0.0, 0.0, -self.coordinateUnit, 0.0], [0.0, 0.0, 0.0, self.rotateUnit], [0.0, 0.0, 0.0, -self.rotateUnit]]) """ self.actions = np.array([[self.coordinateUnit, 0.0, 0.0, 0.0], [-self.coordinateUnit, 0.0, 0.0, 0.0], [0.0, 0.0, self.coordinateUnit, 0.0], [0.0, 0.0, -self.coordinateUnit, 0.0]]) self.robotPose = PoseRpy() self.robotState = np.array([0] * 6, dtype=np.float32) self.TCPForce = 0 self.done = False self.initPose = moveToCartesianPoseGoal() """ self.initPose.x = 0.567 self.initPose.y = 0.365 self.initPose.z = 0.416 self.initPose.u = 3.14 self.initPose.v = 0 self.initPose.w = 1.57 """ self.initPose.x = 0.81 self.initPose.y = 0.19 self.initPose.z = -0.0054 self.initPose.u = 3.14 self.initPose.v = 0 self.initPose.w = 1.57 self.goalPose = moveToCartesianPoseGoal() """ self.goalPose.x = 0.567 self.goalPose.y = 0.365 self.goalPose.z = 0.6 self.goalPose.u = 3.14 self.goalPose.v = 0 self.goalPose.w = 1.57 """ self.goalPose.x = 0.4 self.goalPose.y = 0.19 self.goalPose.z = 0.4 self.goalPose.u = -3.14 self.goalPose.v = 0 self.goalPose.w = 1.57 self.initDistance = self.calcDistance(self.initPose, self.goalPose) self.distance = self.calcDistance(self.initPose, self.goalPose) self.startTime = rospy.get_time() self.timeOver = False rospy.loginfo("waiting for server ...") sub = rospy.Subscriber('/ur_moveit_planner/currentPoseRpy', PoseRpy, self.callback) self.action_client = actionlib.SimpleActionClient( "ur_moveit_planner/moveToCartesianPoseAction", moveToCartesianPoseAction) self.action_client.wait_for_server() rospy.loginfo("server ON") self.reset()
#!/usr/bin/env python # -*- coding: utf-8 -*- import rospy import actionlib from ur_moveit_planner.msg import moveToCartesianPoseAction from ur_moveit_planner.msg import moveToCartesianPoseGoal if __name__ == '__main__': rospy.init_node('action_client') action_client = actionlib.SimpleActionClient("ur_moveit_planner/moveToCartesianPoseAction",moveToCartesianPoseAction) action_client.wait_for_server() rospy.loginfo("server ON") target_pose = moveToCartesianPoseGoal() target_pose.x = 0.81; target_pose.y = 0.19; target_pose.z = 0; target_pose.u = 3.14; target_pose.v = 0; target_pose.w = 1.57; action_client.send_goal(target_pose) finished = action_client.wait_for_result(rospy.Duration(5.0)) if finished: result = action_client.get_state(); print result else: print "time out!!"
def __init__(self): self.rotateUnit = 3.14 / 72 self.actions = np.array([ #[self.rotateUnit, 0.0, 0.0, 0.0, 0.0, 0.0], [-self.rotateUnit, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, self.rotateUnit, 0.0, 0.0, 0.0, 0.0], [0.0, -self.rotateUnit, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, self.rotateUnit, 0.0, 0.0, 0.0], [0.0, 0.0, -self.rotateUnit, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, self.rotateUnit, 0.0, 0.0], [0.0, 0.0, 0.0, -self.rotateUnit, 0.0, 0.0], #[0.0, 0.0, 0.0, 0.0, self.rotateUnit, 0.0], [0.0, 0.0, 0.0, 0.0, -self.rotateUnit, 0.0], #[0.0, 0.0, 0.0, 0.0, 0.0, self.rotateUnit], [0.0, 0.0, 0.0, 0.0, 0.0, -self.rotateUnit] ]) self.robotJointAngles = moveToJointAnglesGoal() self.robotState = np.array([0] * 6, dtype=np.float32) self.TCPForce = 0 self.done = False self.initJointAngles = moveToJointAnglesGoal() self.initJointAngles.a1 = 0.0 / 180 * math.pi self.initJointAngles.a2 = -90.0 / 180 * math.pi self.initJointAngles.a3 = 90.0 / 180 * math.pi self.initJointAngles.a4 = -90.0 / 180 * math.pi self.initJointAngles.a5 = -90.0 / 180 * math.pi self.initJointAngles.a6 = 90.0 / 180 * math.pi self.initPose = moveToCartesianPoseGoal() self.initPose.x = 0.4869 self.initPose.y = 0.1090 self.initPose.z = 0.5140 self.initPose.u = -90.0 / 180 * math.pi self.initPose.v = 0.0 / 180 * math.pi self.initPose.w = 180.0 / 180 * math.pi self.goalPose = moveToCartesianPoseGoal() """ self.goalJointAngles = moveToJointAnglesGoal() self.goalJointAngles.a1 = -0.0171 self.goalJointAngles.a2 = -1.512 self.goalJointAngles.a3 = 1.505 self.goalJointAngles.a4 = 0.007 self.goalJointAngles.a5 = -0.012 self.goalJointAngles.a6 = 0.012 """ self.goalPose = moveToCartesianPoseGoal() self.goalPose.x = 0.55 self.goalPose.y = 0.365 self.goalPose.z = 0.25 self.goalPose.u = -90.0 / 180 * math.pi self.goalPose.v = 0.0 / 180 * math.pi self.goalPose.w = 180.0 / 180 * math.pi self.initDistance = self.calcDistance(self.initPose, self.goalPose) self.distance = self.calcDistance(self.initPose, self.goalPose) self.startTime = rospy.get_time() self.timeOver = False rospy.loginfo("waiting for server ...") _jointAnglesSub = rospy.Subscriber( '/ur_moveit_planner/currentJointAngles', JointQuantity, self.callbackJointAngles) _poseRpySub = rospy.Subscriber('/ur_moveit_planner/currentPoseRpy', PoseRpy, self.callbackPoseRpy) self.action_client = actionlib.SimpleActionClient( "ur_moveit_planner/moveToJointAnglesAction", moveToJointAnglesAction) self.action_client.wait_for_server() rospy.loginfo("server ON") self.reset()