def disable_suction(self):
goal = MoveRobotGoal()
goal.action = "suction_off"
self.client.send_goal(goal)
self.client.wait_for_result()
result = self.client.get_result()
return result.success
def open_gripper(self):
goal = MoveRobotGoal()
goal.action = "open_gripper"
self.client.send_goal(goal)
self.client.wait_for_result()
result = self.client.get_result()
return result.success
def movel(self, pose, acceleration=1.0, velocity=0.2, use_mm=False, max_retries=3):
pose_local = pose.copy()
if use_mm:
pose_local[0] *= 0.001
pose_local[1] *= 0.001
pose_local[2] *= 0.001
pose_local = self.apply_gripper_tcp_offset(pose_local)
pose_local = apply_transform_real_to_moveit(pose_local)
print("moving to " + str(pose_local))
quaternion = Rotation.from_rotvec(pose_local[3:]).as_quat()
goal = MoveRobotGoal()
goal.action = "cartesian"
goal.cartesian_goal.position.x = pose_local[0]
goal.cartesian_goal.position.y = pose_local[1]
goal.cartesian_goal.position.z = pose_local[2]
goal.cartesian_goal.orientation.x = quaternion[0]
goal.cartesian_goal.orientation.y = quaternion[1]
goal.cartesian_goal.orientation.z = quaternion[2]
goal.cartesian_goal.orientation.w = quaternion[3]
retry_amount = 0
success = False
while retry_amount < max_retries:
self.client.send_goal(goal)
self.client.wait_for_result()
result = self.client.get_result()
success = result.success
if success:
break
retry_amount += 1
return success
def movej(self, pose, acceleration=1.0, velocity=0.1, degrees=True, max_retries=3):
pose_local = pose.copy()
if degrees:
for i in range(6):
pose_local[i] = math.radians(pose_local[i])
goal = MoveRobotGoal()
goal.action = "joint"
goal.joint_goal.positions = pose_local
goal.joint_goal.velocities = np.repeat(velocity, 6)
retry_amount = 0
success = False
while retry_amount < max_retries:
self.client.send_goal(goal)
self.client.wait_for_result()
result = self.client.get_result()
success = result.success
if success:
break
retry_amount += 1
return success
def close_gripper(self, width=0, speed=5, lock=False, gripping_box=False):
goal = MoveRobotGoal()
goal.action = "close_gripper"
goal.gripper_width = width
goal.gripper_speed = speed
goal.gripper_lock = lock
goal.gripper_grip_box = gripping_box
self.client.send_goal(goal)
self.client.wait_for_result()
result = self.client.get_result()
return result.success
def run(self):
# goal = MoveRobotGoal()
# goal.action = "joint"
# goal.joint_goal = JointTrajectoryPoint()
# goal.joint_goal.positions = [
# 0,
# 0,
# 0,
# 0,
# 0,
# 0
# ]
# self.interface_client.send_goal(goal)
# self.interface_client.wait_for_result(rospy.Duration(10))
# rospy.loginfo("Sending cartesian goal")
# goal.action = "cartesian"
# goal.cartesian_goal = Pose()
# coordinates = [0.373, -0.0165, 0.257, 2.405, 1.018, 2.52]
# rotation = Rotation.from_rotvec(coordinates[3:]).as_quat()
# goal.cartesian_goal.position.x = coordinates[0]
# goal.cartesian_goal.position.y = coordinates[1]
# goal.cartesian_goal.position.z = coordinates[2]
# goal.cartesian_goal.orientation.x = rotation[0]
# goal.cartesian_goal.orientation.y = rotation[1]
# goal.cartesian_goal.orientation.z = rotation[2]
# goal.cartesian_goal.orientation.w = rotation[3]
# self.interface_client.send_goal(goal)
# self.interface_client.wait_for_result(rospy.Duration(10))
rospy.loginfo("Send move to home goal")
goal = MoveRobotGoal()
goal.action = "home"
self.interface_client.send_goal(goal)
self.interface_client.wait_for_result(rospy.Duration(10))
rospy.loginfo("Finished")