def moveToObject(self):
rospy.logwarn("move to object")
resp = False
end_effector_name = "gripper"
kinematics_pose = KinematicsPose()
planning_group = "arm"
# kinematics_pose.pose = self.pickTargetPose.pose
kinematics_pose.pose = self.pickObjectPose.pose
rospy.loginfo(self.pickObjectPose.pose)
#--------------------------------------------------------------------------#
# 버튼인식 이용 시 출력되는 좌표의 x, z좌표에 +0.05를 해야 정확한 위치로 이동
#--------------------------------------------------------------------------#
kinematics_pose.pose.position = self.forwardObjectPosition(
kinematics_pose.pose.position, 0.05)
kinematics_pose.pose.position.y -= 0.025
kinematics_pose.pose.position.z += 0.025
moveDistance = math.sqrt((kinematics_pose.pose.position.x -
self.currentToolPose.position.x)**2 +
(kinematics_pose.pose.position.y -
self.currentToolPose.position.y)**2 +
(kinematics_pose.pose.position.z -
self.currentToolPose.position.z)**2)
#distance 0.3 m -> 3 sec operate time
#distance 0.1 m -> 1 sec operate time
operating_time = moveDistance * 10
operating_limit_time = operating_time
if operating_time < 1:
operating_limit_time = 1
elif operating_time > 3:
operating_limit_time = 3
rospy.logwarn("go xyz %.2f,%.2f,%.2f , moveDistance %.2f, operate time %.2f ( %.2f )" ,\
kinematics_pose.pose.position.x, kinematics_pose.pose.position.y, kinematics_pose.pose.position.z, \
moveDistance, operating_time , operating_limit_time)
try:
resp = self.set_kinematics_position(planning_group,
end_effector_name,
kinematics_pose,
operating_time)
print('kinemetics resp1 {} time '.format(resp.is_planned,
operating_time))
rospy.sleep(operating_time)
except rospy.ServiceException:
print("Service call failed: %s" % e)
return False
return resp
def talker():
rospy.init_node('OM_publisher') #Initiate a Node called 'OM_publisher'
set_kinematics_position = rospy.ServiceProxy(
'/goal_joint_space_path_to_kinematics_pose', SetKinematicsPose)
set_gripper_position = rospy.ServiceProxy('/goal_tool_control',
SetJointPosition)
#while not rospy.is_shutdown():
kinematics_pose = KinematicsPose()
kinematics_pose.pose.position.x = 0.2867
kinematics_pose.pose.position.y = 0.0
kinematics_pose.pose.position.z = 0.194
kinematics_pose.pose.orientation.w = 0.999
kinematics_pose.pose.orientation.x = 1.765
kinematics_pose.pose.orientation.y = 0.023
kinematics_pose.pose.orientation.z = 0.0
resp1 = set_kinematics_position('planning_group', 'gripper',
kinematics_pose, 3)
gripper_position = JointPosition()
gripper_position.joint_name = ['gripper']
gripper_position.position = [
0.01
] # -0.01 for fully close and 0.01 for fully open
respg2 = set_gripper_position('planning_group', gripper_position, 3)
rospy.sleep(3)
kinematics_pose = KinematicsPose()
kinematics_pose.pose.position.x = 0.0128
kinematics_pose.pose.position.y = 0.2748
kinematics_pose.pose.position.z = 0.1938
kinematics_pose.pose.orientation.w = 0.707
kinematics_pose.pose.orientation.x = -0.016
kinematics_pose.pose.orientation.y = 0.016
kinematics_pose.pose.orientation.z = 0.7058
resp1 = set_kinematics_position('planning_group', 'gripper',
kinematics_pose, 3)
rospy.sleep(3)
gripper_position = JointPosition()
gripper_position.joint_name = ['gripper']
gripper_position.position = [
-0.01
] # -0.01 for fully close and 0.01 for fully open
respg2 = set_gripper_position('planning_group', gripper_position, 3)
def closeToObject(self):
rospy.logwarn("close to object")
resp = False
end_effector_name = "gripper"
kinematics_pose = KinematicsPose()
planning_group = "arm"
kinematics_pose.pose = self.pickTargetPose.pose
kinematics_pose.pose.position = self.forwardObjectPosition( kinematics_pose.pose.position, 0.03 )
if self.use_platform :
rospy.logwarn("??????1")
kinematics_pose.pose.position.z += 0.05
else :
rospy.logwarn("??????2")
kinematics_pose.pose.position.z -= 0.05
kinematics_pose.pose.position.y += 0.005
moveDistance = math.sqrt((kinematics_pose.pose.position.x - self.currentToolPose.position.x)**2
+ (kinematics_pose.pose.position.y - self.currentToolPose.position.y)**2
+ (kinematics_pose.pose.position.z - self.currentToolPose.position.z)**2 )
#distance 0.3 m -> 3 sec operate time
#distance 0.1 m -> 1 sec operate time
operating_time = moveDistance * 10
operating_limit_time = operating_time
if operating_time < 1 :
operating_limit_time = 1
elif operating_time > 3 :
operating_limit_time = 3
rospy.logwarn("go xyz %.2f,%.2f,%.2f , moveDistance %.2f, operate time %.2f ( %.2f )" ,\
kinematics_pose.pose.position.x, kinematics_pose.pose.position.y, kinematics_pose.pose.position.z, \
moveDistance, operating_time , operating_limit_time)
try:
resp = self.set_kinematics_position(planning_group, end_effector_name, kinematics_pose, operating_time)
print 'kinemetics resp1 {} time '.format(resp.is_planned, operating_time)
rospy.sleep(operating_time)
except rospy.ServiceException, e:
print "Service call failed: %s"%e
return False
def test_achieve_goal():
env = OpenManipulatorReacherEnv(cfg)
for iter in range(20):
block_pose = Pose()
block_pose.position.x = np.random.uniform(0.25, 0.6)
block_pose.position.y = np.random.uniform(-0.4, 0.4)
block_pose.position.z = 0.00
block_pose.orientation = overhead_orientation
env.ros_interface.set_target_block(block_pose)
r_pose = Pose()
r_pose.position = block_pose.position
r_pose.position.z = 0.08
forward_pose = KinematicsPose()
forward_pose.pose = r_pose
forward_pose.max_accelerations_scaling_factor = 0.0
forward_pose.max_velocity_scaling_factor = 0.0
forward_pose.tolerance = 0.0
try:
task_space_srv = rospy.ServiceProxy(
"/open_manipulator/goal_task_space_path", SetKinematicsPose)
_ = task_space_srv("arm", "gripper", forward_pose, 2.0)
except rospy.ServiceException as e:
rospy.loginfo("Path planning service call failed: {0}".format(e))
rospy.sleep(5.0)
env.ros_interface.delete_target_block()
def test_workspace_limit():
env = OpenManipulatorReacherEnv(cfg)
for iter in range(100):
_polar_rad = np.random.uniform(0.134, 0.32)
_polar_theta = np.random.uniform(-pi * 0.7 / 4, pi * 0.7 / 4)
block_pose = Pose()
block_pose.position.x = _polar_rad * cos(_polar_theta)
block_pose.position.y = _polar_rad * sin(_polar_theta)
block_pose.position.z = np.random.uniform(0.05, 0.28)
block_pose.orientation = overhead_orientation
env.ros_interface.set_target_block(block_pose)
r_pose = Pose()
r_pose.position = block_pose.position
forward_pose = KinematicsPose()
forward_pose.pose = r_pose
forward_pose.max_accelerations_scaling_factor = 0.0
forward_pose.max_velocity_scaling_factor = 0.0
forward_pose.tolerance = 0.0
try:
task_space_srv = rospy.ServiceProxy(
"/open_manipulator/goal_task_space_path", SetKinematicsPose)
_ = task_space_srv("arm", "gripper", forward_pose, 3.0)
except rospy.ServiceException as e:
rospy.loginfo("Path planning service call failed: {0}".format(e))
rospy.sleep(3.0)
env.ros_interface.check_for_termination()
env.ros_interface.delete_target_block()
def talker():
rospy.init_node('OM_publisher') #Initiate a Node called 'OM_publisher'
set_kinematics_position = rospy.ServiceProxy(
'/goal_joint_space_path_to_kinematics_position', SetKinematicsPose)
set_gripper_position = rospy.ServiceProxy('/goal_tool_control',
SetJointPosition)
#while not rospy.is_shutdown():
kinematics_pose = KinematicsPose()
kinematics_pose.pose.position.x = 0.287
kinematics_pose.pose.position.y = 0.0
kinematics_pose.pose.position.z = 0.194
resp1 = set_kinematics_position('planning_group', 'gripper',
kinematics_pose, 3)
sub_kinematics_pose = rospy.Subscriber('/gripper/kinematics_pose',
KinematicsPose, callback)
gripper_position = JointPosition()
gripper_position.joint_name = ['gripper']
gripper_position.position = [
0.01
] # -0.01 for fully close and 0.01 for fully open
respg2 = set_gripper_position('planning_group', gripper_position, 3)
rospy.sleep(3)
kinematics_pose = KinematicsPose()
kinematics_pose.pose.position.x = 0.0
kinematics_pose.pose.position.y = 0.276
kinematics_pose.pose.position.z = 0.192
resp1 = set_kinematics_position('planning_group', 'gripper',
kinematics_pose, 3)
rospy.sleep(3)
sub_kinematics_pose = rospy.Subscriber('/gripper/kinematics_pose',
KinematicsPose, callback)
def eef_pose_request_cb(userdata, request):
eef = KinematicsPose()
eef.pose = userdata.input_pose
rospy.loginfo('eef.position.x : %f', eef.pose.position.x)
rospy.loginfo('eef.position.y : %f', eef.pose.position.y)
rospy.loginfo('eef.position.z : %f', eef.pose.position.z)
eef.max_velocity_scaling_factor = 1.0
eef.max_accelerations_scaling_factor = 1.0
eef.tolerance = userdata.input_tolerance
request.planning_group = userdata.input_planning_group
request.kinematics_pose = eef
return request
def moveTaskSpace(topic, x, y, z, time):
rospy.wait_for_service('/' + topic + '/goal_task_space_path')
try:
SetTaskSpaceGoal = rospy.ServiceProxy(
'/' + topic + '/goal_task_space_path', SetKinematicsPose)
SetKPose = SetKinematicsPoseRequest()
SetKPose.planning_group = ''
SetKPose.end_effector_name = 'gripper'
SetKPose.path_time = float(time)
kPose = KinematicsPose()
coord = Point(float(x), float(y), float(z))
kPose.pose.position = coord
SetKPose.kinematics_pose = kPose
resp = SetTaskSpaceGoal(SetKPose.planning_group,
SetKPose.end_effector_name,
SetKPose.kinematics_pose, SetKPose.path_time)
except rospy.ServiceException, e:
print "Service call failed: %s" % e
def arm_home(): #arm point left; gripper close
rospy.init_node('OM_publisher') #Initiate a Node called 'OM_publisher'
set_kinematics_position = rospy.ServiceProxy(
'/open_manipulator/goal_joint_space_path_to_kinematics_position',
SetKinematicsPose)
set_gripper_position = rospy.ServiceProxy(
'/open_manipulator/goal_tool_control', SetJointPosition)
kinematics_pose = KinematicsPose()
kinematics_pose.pose.position.x = 0.15
kinematics_pose.pose.position.y = 0.01
kinematics_pose.pose.position.z = 0.2
resp1 = set_kinematics_position('planning_group', 'gripper',
kinematics_pose, 3)
gripper_position = JointPosition()
gripper_position.joint_name = ['gripper']
gripper_position.position = [
0.0
] # -0.01 for fully close and 0.01 for fully open
respg2 = set_gripper_position('planning_group', gripper_position, 3)
rospy.sleep(3)
def test_forward():
env = OpenManipulatorReacherEnv(cfg)
_ = env.reset()
_pose = Pose()
_pose.position.x = 0.4
_pose.position.y = 0.0
_pose.position.z = 0.1
_pose.orientation.x = 0.0
_pose.orientation.y = 0.0
_pose.orientation.z = 0.0
_pose.orientation.w = 1.0
forward_pose = KinematicsPose()
forward_pose.pose = _pose
forward_pose.max_accelerations_scaling_factor = 0.0
forward_pose.max_velocity_scaling_factor = 0.0
forward_pose.tolerance = 0.0
try:
task_space_srv = rospy.ServiceProxy(
"/open_manipulator/goal_task_space_path", SetKinematicsPose)
_ = task_space_srv("arm", "gripper", forward_pose, 2.0)
except rospy.ServiceException as e:
rospy.loginfo("Path planning service call failed: {0}".format(e))
def execute(self, userdata):
print "Send Position by Joint Control "
joint_position = JointPosition()
joint_position.joint_name = ['joint1', 'joint2', 'joint3', 'joint4']
joint_position.position = [0, 0, 0, 0]
planning_group = userdata.input_planning_group
end_effector_name = "gripper"
kinematics_pose = KinematicsPose()
cup_tf = [0, 0, 0]
self.mode = 9
self.step = self.StepCupTracking.waiting_signal.value
while 1:
rospy.logwarn("waiting_signal %d ", )
if rospy.is_shutdown():
return 'aborted'
elif self.mode == 2:
return 'succeeded'
elif self.mode == 6:
self.stop_movement(userdata)
self.step = self.StepCupTracking.waiting_signal.value
pass
if self.step == self.StepCupTracking.waiting_signal.value:
#rospy.logwarn("waiting_signal")
if self.mode == 7:
self.pre_step = self.StepCupTracking.waiting_signal.value
self.step = self.StepCupTracking.go_backward_location.value
else:
self.step = self.StepCupTracking.go_backward_location.value
pass
if self.step == self.StepCupTracking.go_backward_location.value:
rospy.logwarn("go_backward_location")
#rospy.sleep(3)
joint_position.position = [0, -1.551, -0.234, 1.98]
path_time = 2.5
try:
resp1 = self.set_joint_position(planning_group,
joint_position, path_time)
#print 'resp1 {}'.format(resp1.is_planned)
rospy.sleep(path_time)
except rospy.ServiceException, e:
print "Service call failed: %s" % e
self.pre_step = self.StepCupTracking.go_backward_location.value
self.step = self.StepCupTracking.wait_detecting_tf_of_cup.value
elif self.step == self.StepCupTracking.wait_detecting_tf_of_cup.value:
#rospy.logwarn("wait_detecting_tf_of_cup")
rospy.sleep(3)
object_detect_duration = rospy.get_rostime().to_sec(
) - self.last_detect_time.to_sec()
#rospy.logwarn("duration %.2f",object_detect_duration )
tmp_x = self.trans[0]
tmp_y = self.trans[1]
tmp_z = self.trans[2]
distance = math.sqrt(tmp_x**2 + tmp_y**2 + tmp_z**2)
width = self.cupboundingBox.xmax - self.cupboundingBox.xmin
if tmp_z < 0.06:
tmp_z = 0.05
#rospy.sleep(0.5)
if self.use_platform:
if object_detect_duration > 1 or tmp_x > 0.5 or distance > 0.7 or tmp_x < 0.07 or self.cupboundingBox.Z < 0.01:
rospy.logwarn("object_detect_duration1 %.2f,tmp_x %.2f,distance %.2f,tmp_z %.2f,tmp_x %.2f, self.cupboundingBox.Z %.2f", \
object_detect_duration,tmp_x,distance,tmp_z,tmp_x, self.cupboundingBox.Z)
rospy.logwarn(" no object1.. ")
pass
else:
radian = math.atan(tmp_y / tmp_x)
degree = math.degrees(radian)
#dist = 0.1
dist = 0.07
distX = math.cos(radian) * dist
distY = math.sin(radian) * dist
rospy.logwarn(
"tmp_xyz1 %.2f,%.2f,%.2f _ radian %.2f(%.2f) distXY %.3f , %.3f",
tmp_x, tmp_y, tmp_z, radian, degree, distX, distY)
cup_tf = [tmp_x - distX, tmp_y - distY, tmp_z + 0.05]
self.pre_step = self.StepCupTracking.wait_detecting_tf_of_cup.value
self.step = self.StepCupTracking.go_base_location.value
else:
if object_detect_duration > 1 or tmp_x > 0.5 or distance > 0.7 or self.cupboundingBox.Z < 0.01:
rospy.logwarn("object_detect_duration2 %.2f,tmp_x %.2f,distance %.2f,tmp_z %.2f,tmp_x %.2f, self.cupboundingBox.Z %.2f", \
object_detect_duration,tmp_x,distance,tmp_z,tmp_x, self.cupboundingBox.Z)
rospy.logwarn(" no object2.. ")
pass
else:
radian = math.atan(tmp_y / tmp_x)
degree = math.degrees(radian)
dist = 0.038
distX = math.cos(radian) * dist
distY = math.sin(radian) * dist
rospy.logwarn(
"tmp_xyz2 %.2f,%.2f,%.2f _ radian %.2f(%.2f) distXY %.3f , %.3f",
tmp_x, tmp_y, tmp_z, radian, degree, distX, distY)
cup_tf = [tmp_x - distX, tmp_y - distY, tmp_z]
self.pre_step = self.StepCupTracking.wait_detecting_tf_of_cup.value
self.step = self.StepCupTracking.go_base_location.value
path_time = 1
resp = self.set_gripper_control("",joint_position, path_time)
rospy.sleep(path_time)
except rospy.ServiceException, e:
print "Service call failed: %s"%e
if not resp :
return False
# initial pose
self.setBackwardPose2()
# go place position
rospy.logwarn("go to place")
resp = False
end_effector_name = "gripper"
kinematics_pose = KinematicsPose()
planning_group = "arm"
kinematics_pose.pose = self.placeTargetPose.pose
kinematics_pose.pose.position = self.forwardObjectPosition( kinematics_pose.pose.position, 0.03 )
kinematics_pose.pose.position.z += 0.13
kinematics_pose.pose.position.x += 0.015
moveDistance = math.sqrt((kinematics_pose.pose.position.x - self.currentToolPose.position.x)**2
+ (kinematics_pose.pose.position.y - self.currentToolPose.position.y)**2
+ (kinematics_pose.pose.position.z - self.currentToolPose.position.z)**2 )
#distance 0.3 m -> 3 sec operate time
#distance 0.1 m -> 1 sec operate time
operating_time = moveDistance * 10
operating_limit_time = operating_time