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_action(self, act, train=True):
action = self.actions[act]
#print(act, action, self.robotJointAngles)
nextRobotJointAngles = moveToJointAnglesGoal()
currentRobotJointAngles = copy.deepcopy(self.robotJointAngles)
nextRobotJointAngles.a1 = currentRobotJointAngles.a1 + action[0]
nextRobotJointAngles.a2 = currentRobotJointAngles.a2 + action[1]
nextRobotJointAngles.a3 = currentRobotJointAngles.a3 + action[2]
nextRobotJointAngles.a4 = currentRobotJointAngles.a4 + action[3]
nextRobotJointAngles.a5 = currentRobotJointAngles.a5 + action[4]
nextRobotJointAngles.a6 = currentRobotJointAngles.a6 + action[5]
#print(nextRobotJointAngles)
return self.move(nextRobotJointAngles,train)
#!/usr/bin/env python
# -*- coding: utf-8 -*-
import rospy
import math
import actionlib
from ur_moveit_planner.msg import moveToJointAnglesAction
from ur_moveit_planner.msg import moveToJointAnglesGoal
if __name__ == '__main__':
rospy.init_node('action_client_joint')
action_client = actionlib.SimpleActionClient(
"ur_moveit_planner/moveToJointAnglesAction", moveToJointAnglesAction)
action_client.wait_for_server()
rospy.loginfo("server ON")
target_jointAngles = moveToJointAnglesGoal()
target_jointAngles.a1 = 0.0 / 180 * math.pi
target_jointAngles.a2 = -90.0 / 180 * math.pi
target_jointAngles.a3 = 90.0 / 180 * math.pi
target_jointAngles.a4 = -90.0 / 180 * math.pi
target_jointAngles.a5 = -90.0 / 180 * math.pi
target_jointAngles.a6 = 90.0 / 180 * math.pi
action_client.send_goal(target_jointAngles)
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()