def setBackwardPose(self):
rospy.logwarn("setInitPose")
# init position
joint_position = JointPosition()
joint_position.joint_name = ['joint1', 'joint2', 'joint3', 'joint4']
#joint_position.position = [0.0, -1.05, 0.35, 0.70]
joint_position.position = [0.0, -1.791, 0.507, 1.438]
resp = False
try:
path_time = 2
resp = self.set_joint_position("", joint_position, path_time)
rospy.sleep(path_time)
except rospy.ServiceException as e:
print("Service call failed: %s" % e)
if not resp:
return False
# open gripper
joint_position = JointPosition()
joint_position.joint_name = ['gripper']
joint_position.position = [0.01] #-0.01 0.01
resp = False
try:
path_time = 1
resp = self.set_gripper_control("", joint_position, path_time)
rospy.sleep(path_time)
except rospy.ServiceException as e:
print("Service call failed: %s" % e)
if not resp:
return False
return True
def execute(self, userdata):
inputfile = userdata.input_motionfile_file
self.count = 0
while 1:
if rospy.is_shutdown():
break
if self.count > (len(inputfile) - 1):
break
if self.count == 0:
position_path_time = 2
gripper_path_time = 1
operating_time = 2
else:
position_path_time = 0.5
gripper_path_time = 0.5
operating_time = 0.2
try:
joint_position = JointPosition()
planning_group = userdata.input_planning_group
joint_position.joint_name = [
'joint1', 'joint2', 'joint3', 'joint4'
]
joint_position.position = [inputfile[self.count][0], inputfile[self.count][1], \
inputfile[self.count][2], inputfile[self.count][3]]
path_time = position_path_time
resp1 = self.set_joint_position(planning_group, joint_position,
position_path_time)
#print 'resp1 {}'.format(resp1.is_planned)
#rospy.sleep(position_path_time)
except rospy.ServiceException, e:
print "Service call failed: %s" % e
try:
gripper_position = JointPosition()
planning_group = userdata.input_planning_group
gripper_position.joint_name = ['gripper']
if self.use_platform:
gripper_position.position = [inputfile[self.count][4]]
else:
if inputfile[self.count][4] < 0.005:
gripper_position.position = [-0.01]
else:
gripper_position.position = [0.01]
path_time = gripper_path_time
resp1 = self.set_gripper_position(planning_group,
gripper_position,
gripper_path_time)
#print 'resp1 {}'.format(resp1.is_planned)
#rospy.sleep(0.2)
except rospy.ServiceException, e:
print "Service call failed: %s" % e
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 main():
rclpy.init(args=sys.argv)
node = rclpy.create_node("iknet_inference")
set_joint_position = node.create_client(SetJointPosition,
"/goal_joint_space_path")
parser = argparse.ArgumentParser()
parser.add_argument(
"--model",
type=str,
default="./iknet.pth",
)
parser.add_argument("--trt", action="store_true", default=False)
parser.add_argument("--x", type=float, default=0.1)
parser.add_argument("--y", type=float, default=0.0)
parser.add_argument("--z", type=float, default=0.1)
parser.add_argument("--qx", type=float, default=0.0)
parser.add_argument("--qy", type=float, default=0.0)
parser.add_argument("--qz", type=float, default=0.0)
parser.add_argument("--qw", type=float, default=1.0)
args = parser.parse_args()
device = torch.device("cuda" if torch.cuda.is_available() else "cpu")
if not args.trt:
model = IKNet()
else:
from torch2trt import TRTModule
model = TRTModule()
model.to(device)
model.load_state_dict(torch.load(args.model))
model.eval()
pose = [args.x, args.y, args.z, args.qx, args.qy, args.qz, args.qw]
if not args.trt:
input_ = torch.FloatTensor(pose)
else:
input_ = torch.FloatTensor([pose])
input_ = input_.to(device)
print(f"input: {input_}")
output = model(input_)
print(f"output: {output}")
joint_position = JointPosition()
joint_position.joint_name = [f"joint{i+1}" for i in range(4)]
if not args.trt:
joint_position.position = [output[i].item() for i in range(4)]
else:
joint_position.position = [output[0][i].item() for i in range(4)]
request = SetJointPosition.Request()
request.joint_position = joint_position
request.path_time = 4.0
future = set_joint_position.call_async(request)
rclpy.spin_until_future_complete(node, future)
if future.result() is not None:
print(f"result: {future.result().is_planned}")
else:
print(f"exception: {future.exception()}")
node.destroy_node()
rclpy.shutdown()
def stop_movement(self, userdata):
joint_position = JointPosition()
joint_position.joint_name = ['joint1','joint2','joint3','joint4']
joint_position.position = [0, 0, 0, 0]
planning_group = userdata.input_planning_group
try:
path_time = 1
resp1 = self.set_joint_position_from_present(planning_group,joint_position, path_time)
rospy.sleep(path_time)
except rospy.ServiceException, e:
print "Service call failed: %s"%e
def arm_home(): #arm point left; gripper half close
rospy.init_node('OM_publisher') #Initiate a Node called 'OM_publisher'
set_joint_position = rospy.ServiceProxy(
'/open_manipulator/goal_joint_space_path', SetJointPosition)
set_gripper_position = rospy.ServiceProxy(
'/open_manipulator/goal_tool_control', SetJointPosition)
joint_position = JointPosition()
joint_position.joint_name = ['joint1', 'joint2', 'joint3', 'joint4']
joint_position.position = [0, -1.052, 0.377, 0.709] # in radians
resp1 = set_joint_position('planning_group', joint_position, 3)
gripper_position = JointPosition()
gripper_position.joint_name = ['gripper']
gripper_position.position = [
0.00
] # -0.01 for fully close and 0.01 for fully open
respg2 = set_gripper_position('planning_group', gripper_position, 3)
rospy.sleep(3)
def talker():
rospy.init_node('OM_publisher') #Initiate a Node called 'OM_publisher'
set_joint_position = rospy.ServiceProxy('/goal_joint_space_path',
SetJointPosition)
set_gripper_position = rospy.ServiceProxy('/goal_tool_control',
SetJointPosition)
while not rospy.is_shutdown():
joint_position = JointPosition()
joint_position.joint_name = ['joint1', 'joint2', 'joint3', 'joint4']
joint_position.position = [-0.5, 0, 0.5, -0.5] # in radians
resp1 = set_joint_position('planning_group', joint_position, 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)
sub_joint_state = rospy.Subscriber('/joint_states', JointState,
callback)
def test_rotate():
_qpose = JointPosition()
_qpose.joint_name = ["joint1", "joint2", "joint3", "joint4"]
_qpose.position = [0.5, 0.0, 0.0, 0.5]
_qpose.max_accelerations_scaling_factor = 0.0
_qpose.max_velocity_scaling_factor = 0.0
try:
task_space_srv = rospy.ServiceProxy(
"/open_manipulator/goal_joint_space_path_from_present", SetJointPosition
)
_ = task_space_srv("arm", _qpose, 2.0)
except rospy.ServiceException, e:
rospy.loginfo("Path planning service call failed: {0}".format(e))
def initAction(self,userdata):
try:
joint_position = JointPosition()
planning_group = userdata.input_planning_group
joint_position.joint_name = ['joint1','joint2','joint3','joint4']
joint_position.position = [0.0, -1.05, 0.35, 0.70]
path_time = 5
resp1 = self.set_joint_position(planning_group,joint_position, path_time)
#print 'resp1 {}'.format(resp1.is_planned)
rospy.sleep(5)
except rospy.ServiceException, e:
print "Service call failed: %s"%e
def setBackwardPose2(self):
rospy.logwarn("setInitPose")
# init position
joint_position = JointPosition()
joint_position.joint_name = ['joint1','joint2','joint3','joint4']
#joint_position.position = [0.0, -1.05, 0.35, 0.70]
joint_position.position = [0.0, -1.791, 0.507, 1.438]
resp = False
try:
path_time = 2
resp = self.set_joint_position("",joint_position, path_time)
rospy.sleep(path_time)
except rospy.ServiceException, e:
print "Service call failed: %s"%e
def moveToPlace(self):
rospy.logwarn("move_to_place")
# close gripper
joint_position = JointPosition()
joint_position.joint_name = ['gripper']
joint_position.position = [-0.01] #-0.01 0.01
resp = False
try:
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
def panoramaShot(self,userdata):
print 'panoramaShot init point\n'
try:
joint_position = JointPosition()
planning_group = userdata.input_planning_group
joint_position.joint_name = ['joint1','joint2','joint3','joint4']
joint_position.position = [-1.57, -1.049, 0.354, 0.715]
path_time = 5
resp1 = self.set_joint_position(planning_group,joint_position, path_time)
#print 'resp1 {}'.format(resp1.is_planned)
rospy.sleep(5)
except rospy.ServiceException, e:
print "Service call failed: %s"%e
def facetrackingActionByJoint(self,userdata):
#center_x = 112 #320
center_x = 182 #320
center_y = 158 #240
x_diff = (center_x - self.face_center_x)
y_diff = (center_y - self.face_center_y)
#move_x = (x_diff*1000/320)*0.1/1000
#move_y = (y_diff*1000/320)*-0.05/1000
move_x = (x_diff*1000/320)*0.3/1000
move_y = (y_diff*1000/320)*-0.15/1000
#rospy.logwarn('C_XY %.3f,%.3f , XY_Diff %.3f,%.3f move_xy %.3f,%.3f)', self.face_center_x ,self.face_center_y , \
#x_diff, y_diff, move_x, move_y)
object_detect_duration = rospy.get_rostime().to_sec() - self.last_detect_time.to_sec()
#rospy.logwarn("duration %.2f",object_detect_duration )
#check last face detected time
if object_detect_duration > 1 :
rospy.logwarn(" no face.. %d ", self.no_detect_count)
self.last_no_detect_time = rospy.get_rostime()
self.no_detect_count = self.no_detect_count + 1
if self.no_detect_count > 30 :
rospy.logwarn(" no_detect_count too long..........................set init!!!!!!!!!!!!!!!!!")
print 'set init position for face tracking \n'
try:
joint_position = JointPosition()
planning_group = userdata.input_planning_group
joint_position.joint_name = ['joint1','joint2','joint3','joint4']
joint_position.position = [0.0, -1.05, 0.35, 0.70]
path_time = 5
resp1 = self.set_joint_position(planning_group,joint_position, path_time)
#print 'resp1 {}'.format(resp1.is_planned)
rospy.sleep(5)
except rospy.ServiceException, e:
print "Service call failed: %s"%e
self.no_detect_count = 0
rospy.sleep(0.1)
return ;
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 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 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
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)
rospy.logwarn( 'kinemetics resp1 {} time '.format(resp.is_planned, operating_time))
rospy.sleep(operating_time)
except rospy.ServiceException, e:
rospy.logwarn( "Service call failed: %s"%e)
return False
# open gripper
joint_position = JointPosition()
joint_position.joint_name = ['gripper']
joint_position.position = [0.01] #-0.01 0.01
resp = False
try:
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
return True
def forwardObjectPosition( self, objectPosition, forward_distance ):
resultPoint = Point()
def talker():
rospy.init_node('OM_publisher') #Initiate a Node called 'OM_publisher'
set_joint_position = rospy.ServiceProxy('/goal_joint_space_path',
SetJointPosition)
set_gripper_position = rospy.ServiceProxy('/goal_tool_control',
SetJointPosition)
joint_position = JointPosition()
joint_position.joint_name = ['joint1', 'joint2', 'joint3', 'joint4']
#Step 1
joint_position.position = [0, 0, 0, 0] # in radians
resp1 = set_joint_position('planning_group', joint_position, 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)
#Step 2
joint_position.position = [1.571, 0, 0, 0] # in radians
resp1 = set_joint_position('planning_group', joint_position, 3)
rospy.sleep(3)
#Step 3
joint_position.position = [1.571, 0.526, 0, 0] # in radians
resp1 = set_joint_position('planning_group', joint_position, 3)
rospy.sleep(3)
#Step 4
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)
#Step 5
joint_position.position = [1.571, 0, 0, 0] # in radians
resp1 = set_joint_position('planning_group', joint_position, 3)
rospy.sleep(3)
#Step 6
joint_position.position = [0, 0, 0, 0] # in radians
resp1 = set_joint_position('planning_group', joint_position, 3)
rospy.sleep(3)
#Step 7
joint_position.position = [-1.571, 0, 0, 0] # in radians
resp1 = set_joint_position('planning_group', joint_position, 3)
rospy.sleep(3)
#Step 8
joint_position.position = [-1.571, 0.526, 0, 0] # in radians
resp1 = set_joint_position('planning_group', joint_position, 3)
rospy.sleep(3)
#Step 7b
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)
#bring arm up
joint_position.position = [-1.571, 0, 0, 0] # in radians
resp1 = set_joint_position('planning_group', joint_position, 3)
rospy.sleep(3)
#initial pose
joint_position.position = [0, 0, 0, 0] # in radians
resp1 = set_joint_position('planning_group', joint_position, 3)
def talker():
rospy.init_node('OM_publisher') #Initiate a Node called 'OM_publisher'
set_joint_position = rospy.ServiceProxy('/goal_joint_space_path',
SetJointPosition)
set_gripper_position = rospy.ServiceProxy('/goal_tool_control',
SetJointPosition)
while not rospy.is_shutdown():
ii = 0
while ii < 50:
ii += 1
joint_position = JointPosition()
joint_position.joint_name = [
'joint1', 'joint2', 'joint3', 'joint4'
]
#joint_position.position = [1, 0.2761, 0.4502, -0.3958] # in radians
#joint_position.position = [3.1416, 0.8674, 0.1562, -1.2435] # in radians
joint_position.position = [0.78, -0.5, 0, 0] # in radians
resp1 = set_joint_position('planning_group', joint_position, 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)
sub_joint_state = rospy.Subscriber('/joint_states', JointState,
callback)
#rospy.sleep(0.1)
while ii < 250:
ii += 1
joint_position = JointPosition()
joint_position.joint_name = [
'joint1', 'joint2', 'joint3', 'joint4'
]
#joint_position.position = [1, 0.2761, 0.4502, -0.3958] # in radians
#joint_position.position = [3.1416, 0.8674, 0.1562, -1.2435] # in radians
joint_position.position = [1.5706, -0.2, 0, 0] # in radians
resp1 = set_joint_position('planning_group', joint_position, 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)
sub_joint_state = rospy.Subscriber('/joint_states', JointState,
callback)
rospy.sleep(0.03)
ii = 0
while ii < 200:
ii += 1
joint_position = JointPosition()
joint_position.joint_name = [
'joint1', 'joint2', 'joint3', 'joint4'
]
#joint_position.position = [1, 0.2761, 0.4502, -0.3958] # in radians
#joint_position.position = [1.570, 0.8674, 0.1562, -1.2435] # in radians
joint_position.position = [1.570, 0.7639, 0.2141,
-0.9771] # in radians
resp1 = set_joint_position('planning_group', joint_position, 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)
sub_joint_state = rospy.Subscriber('/joint_states', JointState,
callback)
rospy.sleep(0.02)
ii = 0
while ii < 200:
ii += 1
joint_position = JointPosition()
joint_position.joint_name = [
'joint1', 'joint2', 'joint3', 'joint4'
]
#joint_position.position = [1, 0.2761, 0.4502, -0.3958] # in radians
#joint_position.position = [1.570, 0.8674, 0.1562, -1.2435] # in radians
joint_position.position = [1.570, 0.7639, 0.2141,
-0.9771] # in radians
resp1 = set_joint_position('planning_group', joint_position, 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)
sub_joint_state = rospy.Subscriber('/joint_states', JointState,
callback)
rospy.sleep(0.01)
ii = 0
while ii < 200:
ii += 1
joint_position = JointPosition()
joint_position.joint_name = [
'joint1', 'joint2', 'joint3', 'joint4'
]
#joint_position.position = [1, 0.2761, 0.4502, -0.3958] # in radians
joint_position.position = [1.570, -0.5, 0, 0] # in radians
resp1 = set_joint_position('planning_group', joint_position, 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)
sub_joint_state = rospy.Subscriber('/joint_states', JointState,
callback)
rospy.sleep(0.001)
joint_position = JointPosition()
joint_position.joint_name = ['joint1', 'joint2', 'joint3', 'joint4']
#joint_position.position = [1, 0.2761, 0.4502, -0.3958] # in radians
joint_position.position = [0, -0.5, 0, 0] # in radians
resp1 = set_joint_position('planning_group', joint_position, 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)
sub_joint_state = rospy.Subscriber('/joint_states', JointState,
callback)
rospy.sleep(0.001)
ii = 0
while ii < 100:
ii += 1
joint_position = JointPosition()
joint_position.joint_name = [
'joint1', 'joint2', 'joint3', 'joint4'
]
#joint_position.position = [1, 0.2761, 0.4502, -0.3958] # in radians
joint_position.position = [-1.57, -0.5, 0, 0] # in radians
resp1 = set_joint_position('planning_group', joint_position, 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)
sub_joint_state = rospy.Subscriber('/joint_states', JointState,
callback)
rospy.sleep(0.1)
ii = 0
while ii < 100:
ii += 1
joint_position = JointPosition()
joint_position.joint_name = [
'joint1', 'joint2', 'joint3', 'joint4'
]
#joint_position.position = [1, 0.2761, 0.4502, -0.3958] # in radians
#joint_position.position = [-1.57, -0.5, 0, 0] # in radians
joint_position.position = [-1.570, 0.7639, 0.2141,
-0.9771] # in radians
resp1 = set_joint_position('planning_group', joint_position, 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)
sub_joint_state = rospy.Subscriber('/joint_states', JointState,
callback)
rospy.sleep(0.02)
ii = 0
while ii < 200:
ii += 1
joint_position = JointPosition()
joint_position.joint_name = [
'joint1', 'joint2', 'joint3', 'joint4'
]
#joint_position.position = [1, 0.2761, 0.4502, -0.3958] # in radians
#joint_position.position = [-1.57, 0.8674, 0.1562, -1.2435] # in radians
joint_position.position = [-1.570, 0.7639, 0.2141,
-0.9771] # in radians
resp1 = set_joint_position('planning_group', joint_position, 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)
sub_joint_state = rospy.Subscriber('/joint_states', JointState,
callback)
rospy.sleep(0.1)
ii = 0
while ii < 100:
ii += 1
joint_position = JointPosition()
joint_position.joint_name = [
'joint1', 'joint2', 'joint3', 'joint4'
]
#joint_position.position = [1, 0.2761, 0.4502, -0.3958] # in radians
#joint_position.position = [-1.57, -0.5, 0, 0] # in radians
joint_position.position = [-1.570, 0.7639, 0.2141,
-0.9771] # in radians
resp1 = set_joint_position('planning_group', joint_position, 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)
sub_joint_state = rospy.Subscriber('/joint_states', JointState,
callback)
rospy.sleep(0.001)
ii = 0
while ii < 100:
ii += 1
joint_position = JointPosition()
joint_position.joint_name = [
'joint1', 'joint2', 'joint3', 'joint4'
]
#joint_position.position = [1, 0.2761, 0.4502, -0.3958] # in radians
joint_position.position = [-1.57, -0.5, 0, 0] # in radians
resp1 = set_joint_position('planning_group', joint_position, 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)
sub_joint_state = rospy.Subscriber('/joint_states', JointState,
callback)
rospy.sleep(0.001)
joint_position = JointPosition()
joint_position.joint_name = ['joint1', 'joint2', 'joint3', 'joint4']
#joint_position.position = [1, 0.2761, 0.4502, -0.3958] # in radians
joint_position.position = [0, -0.5, 0, 0] # in radians
resp1 = set_joint_position('planning_group', joint_position, 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)
sub_joint_state = rospy.Subscriber('/joint_states', JointState,
callback)
rospy.sleep(0.01)
ii = 0
while ii < 200:
ii += 1
joint_position = JointPosition()
joint_position.joint_name = [
'joint1', 'joint2', 'joint3', 'joint4'
]
#joint_position.position = [1, 0.2761, 0.4502, -0.3958] # in radians
joint_position.position = [1.570, -0.5, 0, 0] # in radians
resp1 = set_joint_position('planning_group', joint_position, 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)
sub_joint_state = rospy.Subscriber('/joint_states', JointState,
callback)
rospy.sleep(0.001)
ii = 0
while ii < 100:
ii += 1
joint_position = JointPosition()
joint_position.joint_name = [
'joint1', 'joint2', 'joint3', 'joint4'
]
#joint_position.position = [1, 0.2761, 0.4502, -0.3958] # in radians
joint_position.position = [1.570, 0.7639, 0.2141,
-0.9] # in radians
resp1 = set_joint_position('planning_group', joint_position, 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)
sub_joint_state = rospy.Subscriber('/joint_states', JointState,
callback)
rospy.sleep(0.01)
ii = 0
while ii < 200:
ii += 1
joint_position = JointPosition()
joint_position.joint_name = [
'joint1', 'joint2', 'joint3', 'joint4'
]
#joint_position.position = [1, 0.2761, 0.4502, -0.3958] # in radians
joint_position.position = [1.570, -0.5, 0, 0] # in radians
resp1 = set_joint_position('planning_group', joint_position, 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)
sub_joint_state = rospy.Subscriber('/joint_states', JointState,
callback)
rospy.sleep(0.01)
ii = 0
while ii < 200:
ii += 1
joint_position = JointPosition()
joint_position.joint_name = [
'joint1', 'joint2', 'joint3', 'joint4'
]
#joint_position.position = [1, 0.2761, 0.4502, -0.3958] # in radians
joint_position.position = [0, -0.5, 0, 0] # in radians
resp1 = set_joint_position('planning_group', joint_position, 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)
sub_joint_state = rospy.Subscriber('/joint_states', JointState,
callback)
rospy.sleep(0.01)
ii = 0
while ii < 200:
ii += 1
joint_position = JointPosition()
joint_position.joint_name = [
'joint1', 'joint2', 'joint3', 'joint4'
]
#joint_position.position = [1, 0.2761, 0.4502, -0.3958] # in radians
joint_position.position = [-1.570, -0.5, 0, 0] # in radians
resp1 = set_joint_position('planning_group', joint_position, 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)
sub_joint_state = rospy.Subscriber('/joint_states', JointState,
callback)
rospy.sleep(0.001)
ii = 0
while ii < 100:
ii += 1
joint_position = JointPosition()
joint_position.joint_name = [
'joint1', 'joint2', 'joint3', 'joint4'
]
#joint_position.position = [1, 0.2761, 0.4502, -0.3958] # in radians
joint_position.position = [-1.570, 0.7639, 0.2141,
-0.9] # in radians
resp1 = set_joint_position('planning_group', joint_position, 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)
sub_joint_state = rospy.Subscriber('/joint_states', JointState,
callback)
rospy.sleep(0.01)
ii = 0
while ii < 200:
ii += 1
joint_position = JointPosition()
joint_position.joint_name = [
'joint1', 'joint2', 'joint3', 'joint4'
]
#joint_position.position = [1, 0.2761, 0.4502, -0.3958] # in radians
joint_position.position = [-1.570, -0.5, 0, 0] # in radians
resp1 = set_joint_position('planning_group', joint_position, 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)
sub_joint_state = rospy.Subscriber('/joint_states', JointState,
callback)
rospy.sleep(0.01)
ii = 0
while ii < 200:
ii += 1
joint_position = JointPosition()
joint_position.joint_name = [
'joint1', 'joint2', 'joint3', 'joint4'
]
#joint_position.position = [1, 0.2761, 0.4502, -0.3958] # in radians
joint_position.position = [0, -0.5, 0, 0] # in radians
resp1 = set_joint_position('planning_group', joint_position, 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)
sub_joint_state = rospy.Subscriber('/joint_states', JointState,
callback)
rospy.sleep(0.01)
ii = 0
while ii < 200:
ii += 1
joint_position = JointPosition()
joint_position.joint_name = [
'joint1', 'joint2', 'joint3', 'joint4'
]
#joint_position.position = [1, 0.2761, 0.4502, -0.3958] # in radians
joint_position.position = [1.57, -0.5, 0, 0] # in radians
resp1 = set_joint_position('planning_group', joint_position, 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)
sub_joint_state = rospy.Subscriber('/joint_states', JointState,
callback)
rospy.sleep(0.001)
ii = 0
while ii < 100:
ii += 1
joint_position = JointPosition()
joint_position.joint_name = [
'joint1', 'joint2', 'joint3', 'joint4'
]
#joint_position.position = [1, 0.2761, 0.4502, -0.3958] # in radians
joint_position.position = [1.570, 0.7639, 0.2141,
-0.9] # in radians
resp1 = set_joint_position('planning_group', joint_position, 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)
sub_joint_state = rospy.Subscriber('/joint_states', JointState,
callback)
ii = 0
while ii < 200:
ii += 1
joint_position = JointPosition()
joint_position.joint_name = [
'joint1', 'joint2', 'joint3', 'joint4'
]
#joint_position.position = [1, 0.2761, 0.4502, -0.3958] # in radians
joint_position.position = [1.57, -0.5, 0, 0] # in radians
resp1 = set_joint_position('planning_group', joint_position, 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)
sub_joint_state = rospy.Subscriber('/joint_states', JointState,
callback)
ii = 0
while ii < 200:
ii += 1
joint_position = JointPosition()
joint_position.joint_name = [
'joint1', 'joint2', 'joint3', 'joint4'
]
#joint_position.position = [1, 0.2761, 0.4502, -0.3958] # in radians
joint_position.position = [0, -0.5, 0, 0] # in radians
resp1 = set_joint_position('planning_group', joint_position, 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)
sub_joint_state = rospy.Subscriber('/joint_states', JointState,
callback)
ii = 0
while ii < 200:
ii += 1
joint_position = JointPosition()
joint_position.joint_name = [
'joint1', 'joint2', 'joint3', 'joint4'
]
#joint_position.position = [1, 0.2761, 0.4502, -0.3958] # in radians
joint_position.position = [-1.57, -0.5, 0, 0] # in radians
resp1 = set_joint_position('planning_group', joint_position, 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)
sub_joint_state = rospy.Subscriber('/joint_states', JointState,
callback)
rospy.sleep(0.001)
ii = 0
while 1: #ii<100:
ii += 1
joint_position = JointPosition()
joint_position.joint_name = [
'joint1', 'joint2', 'joint3', 'joint4'
]
#joint_position.position = [1, 0.2761, 0.4502, -0.3958] # in radians
joint_position.position = [-1.570, 0.7639, 0.2141,
-0.9] # in radians
resp1 = set_joint_position('planning_group', joint_position, 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)
sub_joint_state = rospy.Subscriber('/joint_states', JointState,
callback)
center_range_y = 25
#center range set
if abs(x_diff) > center_range_x or abs(y_diff) > center_range_y :
#operating range set _ x range (-90 degree ~ 90 degree) y range (-45 degree ~ 45 degree)
if (self.jointStates[0] > 1.57 and move_x > 0) or (self.jointStates[0] < -1.57 and move_x < 0 ) :
move_x = 0
#rospy.logwarn("set joint0 state %.3f , move_x %.3f",self.jointStates[0], move_x)
if (self.jointStates[3] > 1.57 and move_y > 0) or (self.jointStates[3] < 0 and move_y < 0 ) :
move_y = 0
#rospy.logwarn("set joint3 state %.3f , move_y %.3f",self.jointStates[3], move_y)
#tracking action
try:
joint_position = JointPosition()
planning_group = userdata.input_planning_group
joint_position.joint_name = ['joint1','joint2','joint3','joint4']
joint_position.position = [move_x, 0, 0, move_y]
resp1 = self.set_joint_position_from_present(planning_group,joint_position, 0.5)
rospy.sleep(0.2)
print 'resp2 {}'.format(resp1.is_planned)
except rospy.ServiceException, e:
print "Service call failed: %s"%e
def panoramaShot(self,userdata):
print 'panoramaShot init point\n'
try:
joint_position = JointPosition()
planning_group = userdata.input_planning_group
joint_position.joint_name = ['joint1','joint2','joint3','joint4']
joint_position.position = [-1.57, -1.049, 0.354, 0.715]
def talker():
rospy.init_node('OM_publisher') #Initiate a Node called 'OM_publisher'
set_joint_position = rospy.ServiceProxy('/goal_joint_space_path',
SetJointPosition)
set_gripper_position = rospy.ServiceProxy('/goal_tool_control',
SetJointPosition)
# Loop until ^c; comment this line if you want the arm to stop at the last joint set
#while not rospy.is_shutdown():
joint_position = JointPosition()
joint_position.joint_name = ['joint1', 'joint2', 'joint3', 'joint4']
#Step 1
joint_position.position = [0, 0, 0, 0] # in radians
resp1 = set_joint_position('planning_group', joint_position, 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)
#Step 2
joint_position.position = [1.571, 0, 0, 0] # in radians
resp1 = set_joint_position('planning_group', joint_position, 3)
rospy.sleep(3)
#Step 3a
joint_position.position = [1.571, 0.526, 0, 0] # in radians
resp1 = set_joint_position('planning_group', joint_position, 3)
rospy.sleep(3)
#Step 3b
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)
#Step 4
joint_position.position = [1.571, 0, 0, 0] # in radians
resp1 = set_joint_position('planning_group', joint_position, 3)
rospy.sleep(3)
#Step 5
joint_position.position = [0, 0, 0, 0] # in radians
resp1 = set_joint_position('planning_group', joint_position, 3)
rospy.sleep(3)
#Step 6
joint_position.position = [-1.571, 0, 0, 0] # in radians
resp1 = set_joint_position('planning_group', joint_position, 3)
rospy.sleep(3)
#Step 7a
joint_position.position = [-1.571, 0.526, 0, 0] # in radians
resp1 = set_joint_position('planning_group', joint_position, 3)
rospy.sleep(3)
#Step 7b
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)
#bring arm up
joint_position.position = [-1.571, 0, 0, 0] # in radians
resp1 = set_joint_position('planning_group', joint_position, 3)
rospy.sleep(3)
#initial pose
joint_position.position = [0, 0, 0, 0] # in radians
resp1 = set_joint_position('planning_group', joint_position, 3)
