def __init__(self, router, proportional_gain=2.0):
self.proportional_gain = proportional_gain
self.router = router
# Create base client using TCP router
self.base = BaseClient(self.router)
def main():
# Import the utilities helper module
sys.path.insert(0, os.path.join(os.path.dirname(__file__), ".."))
import utilities
# Parse arguments
args = utilities.parseConnectionArguments()
# Create connection to the device and get the router
with utilities.DeviceConnection.createTcpConnection(args) as router:
# Create required services
base = BaseClient(router)
base_cyclic = BaseCyclicClient(router)
# Example core
success = True
success &= example_move_to_home_position(base)
success &= example_cartesian_action_movement(base, base_cyclic)
success &= example_angular_action_movement(base)
success &= example_move_to_home_position(base)
success &= example_cartesian_trajectory_movement(base, base_cyclic)
success &= example_angular_trajectory_movement(base)
return 0 if success else 1
def __init__(self, action_scale=0.05, target_range=0.15, distance_threshold=0.05, speed=0.10):
self.connection = utilities.DeviceConnection.createTcpConnection()
self.router = self.connection.__enter__()
self.base = BaseClient(self.router)
self.base_cyclic = BaseCyclicClient(self.router)
self.webcam = cv2.VideoCapture(0)
self.action_scale = action_scale
self.target_range = target_range
self.distance_threshold = distance_threshold
self.speed = speed
self.observation_space = gym.spaces.Dict(
observation=gym.spaces.Box(-np.inf, np.inf, self._get_obs().shape),
achieved_goal=gym.spaces.Box(-np.inf, np.inf, (3,)),
desired_goal=gym.spaces.Box(-np.inf, np.inf, (3,)),
)
self.action_space = gym.spaces.Box(-1., 1., (3,))
self.reward_range = (-np.inf, 0)
self.unwrapped = self
self.spec = None
self._set_to_home()
self.init_xyz = self._get_obs()[:3]
self.goal = None
self.sample_goal()
assert self.goal is not None
def main():
# Import the utilities helper module
sys.path.insert(0, os.path.join(os.path.dirname(__file__), ".."))
import utilities
# Parse arguments
args = utilities.parseConnectionArguments()
# Create connection to the device and get the router
with utilities.DeviceConnection.createTcpConnection(args) as router:
# Create required services
base = BaseClient(router)
base_cyclic = BaseCyclicClient(router)
# Example core
success = True
success &= example_move_to_home_position(base)
success &= example_cartesian_action_movement(base, base_cyclic)
success &= example_angular_action_movement(base)
# You can also refer to the 110-Waypoints examples if you want to execute
# a trajectory defined by a series of waypoints in joint space or in Cartesian space
return 0 if success else 1
def __init__(self, router, router_real_time):
self.camera_mtr = None
self.world3D_list = []
self.camera3D_list = []
self.camera3D = None
self.camera3D_count = -100
self.world3D_count = 0
self.joint_angle_list = None
device_manager = DeviceManagerClient(router)
self.actuator_config = ActuatorConfigClient(router)
self.base = BaseClient(router)
self.base_cyclic = BaseCyclicClient(router_real_time)
self.base_feedback = BaseCyclic_pb2.Feedback()
self.gripper = GripperController(router, router_real_time)
def __init__(self, router, ip_address):
self.router = router
self.base_ip_address = ip_address
# Create services
self.base = BaseClient(self.router)
self.device_manager = DeviceManagerClient(self.router)
self.interconnect_config = InterconnectConfigClient(self.router)
self.interconnect_device_id = self.GetDeviceIdFromDevType(
Common_pb2.INTERCONNECT, 0)
if (self.interconnect_device_id is None):
print(
"Could not find the Interconnect in the device list, exiting..."
)
sys.exit(0)
def main():
# Import the utilities helper module
sys.path.insert(0, os.path.join(os.path.dirname(__file__), ".."))
import utilities
# Parse arguments
args = utilities.parseConnectionArguments()
# Create connection to the device and get the router
with utilities.DeviceConnection.createTcpConnection(args) as router:
# Create required services
base = BaseClient(router)
# Example core
# Move to initial position
move_to_home_position(base)
# Move without the protection zone
print_protection_zones(base)
move_in_front_of_protection_zone(base)
move_to_protection_zone(base)
move_to_home_position(base)
# Move with the protection zone
print_protection_zones(base)
move_in_front_of_protection_zone(base)
# Add the protection zone
handle = create_protection_zone(base)
move_to_protection_zone(base)
move_to_home_position(base)
# Delete the protection zone
base.DeleteProtectionZone(handle)
# Print final protection zones
# The example protection zone should be removed
print_protection_zones(base)
def main():
# Import the utilities helper module
sys.path.insert(0, os.path.join(os.path.dirname(__file__), ".."))
import utilities
# Parse arguments
args = utilities.parseConnectionArguments()
# Create connection to the device and get the router
with utilities.DeviceConnection.createTcpConnection(args) as router:
# Create required services
base = BaseClient(router)
# Example core
success = True
success &= example_forward_kinematics(base)
success &= example_inverse_kinematics(base)
return 0 if success else 1
def __init__(self, router, router_real_time):
# Maximum allowed waiting time during actions (in seconds)
self.ACTION_TIMEOUT_DURATION = 20
self.torque_amplification = 2.0 # Torque measure on last actuator is sent as a command to first actuator
# Create required services
device_manager = DeviceManagerClient(router)
self.actuator_config = ActuatorConfigClient(router)
self.base = BaseClient(router)
self.base_cyclic = BaseCyclicClient(router_real_time)
self.base_command = BaseCyclic_pb2.Command()
self.base_feedback = BaseCyclic_pb2.Feedback()
self.base_custom_data = BaseCyclic_pb2.CustomData()
# Detect all devices
device_handles = device_manager.ReadAllDevices()
self.actuator_count = self.base.GetActuatorCount().count
# Only actuators are relevant for this example
for handle in device_handles.device_handle:
if handle.device_type == Common_pb2.BIG_ACTUATOR or handle.device_type == Common_pb2.SMALL_ACTUATOR:
self.base_command.actuators.add()
self.base_feedback.actuators.add()
# Change send option to reduce max timeout at 3ms
self.sendOption = RouterClientSendOptions()
self.sendOption.andForget = False
self.sendOption.delay_ms = 0
self.sendOption.timeout_ms = 3
self.cyclic_t_end = 30 #Total duration of the thread in seconds. 0 means infinite.
self.cyclic_thread = {}
self.kill_the_thread = False
self.already_stopped = False
self.cyclic_running = False
def wave():
elapsed_time.value = 0
global e
global args
if not args:
args = utilities.parseConnectionArguments()
with utilities.DeviceConnection.createTcpConnection(
args) as router: #kann das "with" weg? Nein
# Create required services
base = BaseClient(router)
base_cyclic = BaseCyclicClient(router)
device_manager = DeviceManagerClient(router)
#Activate the continuous auto-focus of the camera
vision_config = VisionConfigClient(router)
vision_device_id = vision_action.example_vision_get_device_id(
device_manager)
# if vision_device_id != 0:
# vision_action.autofocus_action(vision_config, vision_device_id,action_id=2)
# check if robot is not in transport position, if so, move safely
if functions_.pose_comparison(
transport_pos, base_cyclic,
"joint") == True: #Robot in transport position?
highlevel_movements.angular_action_movement(
e,
base,
base_cyclic,
joint_positions=pre_transport_pos,
speed=speedj) #pre transport safety pose
#Wave Sequence
wave_speed = 70 #degrees/sec
# wave_start_pose=[180, 0, 0, 0, 90, 90, 270] #joint4
# wave_left_pose=list(map(add,wave_start_pose,[0,0,0,30,0,0,-30])) #joint4
# wave_start_pose=[180, 0, 0, 0, 0, 0, 0] #joint5
# wave_left_pose=list(map(add,wave_start_pose,[0,0,0,0,0,40,0])) #joint4
# wave_start_pose=[182.49, 36.45, 0.78, 269.14, 2.33, 56.02, 0.11] #joint5
# wave_left_pose=list(map(add,wave_start_pose,[0,0,0,0,0,40,0])) #joint4
wave_start_pose = [182.9, 320.18, 177.58, 260.17, 1.06, 65.48,
180.64] #joint5
wave_left_pose = list(map(add, wave_start_pose,
[0, 0, 0, 0, 0, 40, 0])) #joint4
# highlevel_movements.angular_action_movement(e,base,base_cyclic,joint_positions=wave_start_pose,speed=speedj)
# sys.exit()
highlevel_movements.angular_action_movement(
e, base, base_cyclic, joint_positions=wave_left_pose,
speed=speedj) #okay
#time.sleep(1)
highlevel_movements.sinus_move(e,
v_max=wave_speed,
degrees_move=60,
joint_id=5,
direction=1,
base=base,
base_cyclic=base_cyclic)
time.sleep(0.4) #must be because else meassurements are wrong
highlevel_movements.sinus_move(e,
v_max=wave_speed,
degrees_move=60,
joint_id=5,
direction=-1,
base=base,
base_cyclic=base_cyclic)
time.sleep(0.4) #must be because else meassurements are wrong
highlevel_movements.sinus_move(e,
v_max=wave_speed,
degrees_move=60,
joint_id=5,
direction=1,
base=base,
base_cyclic=base_cyclic)
time.sleep(0.4) #must be because else meassurements are wrong
highlevel_movements.sinus_move(e,
v_max=wave_speed,
degrees_move=60,
joint_id=5,
direction=-1,
base=base,
base_cyclic=base_cyclic)
# time.sleep(0.4)
# highlevel_movements.sinus_move(e,v_max=wave_speed,degrees_move=60,joint_id=5,direction=1,base=base,base_cyclic=base_cyclic)
# time.sleep(0.4) #must be because else meassurements are wrong
# highlevel_movements.sinus_move(e,v_max=wave_speed,degrees_move=60,joint_id=5,direction=-1,base=base,base_cyclic=base_cyclic)
# time.sleep(0.4) #must be because else meassurements are wrong
# highlevel_movements.sinus_move(e,v_max=wave_speed,degrees_move=60,joint_id=5,direction=1,base=base,base_cyclic=base_cyclic)
# time.sleep(0.4) #must be because else meassurements are wrong
# highlevel_movements.sinus_move(e,v_max=wave_speed,degrees_move=60,joint_id=5,direction=-1,base=base,base_cyclic=base_cyclic)
elapsed_time.value = -2
time.sleep(0.4)
highlevel_movements.angular_action_movement(
e,
base,
base_cyclic,
joint_positions=pre_transport_pos,
speed=speedj) #pre transport safety pose
highlevel_movements.angular_action_movement(
e, base, base_cyclic, joint_positions=transport_pos,
speed=speedj) #transport pose
def __init__(self, router, router_real_time, proportional_gain=2.0):
"""
GripperLowLevelExample class constructor.
Inputs:
kortex_api.RouterClient router: TCP router
kortex_api.RouterClient router_real_time: Real-time UDP router
float proportional_gain: Proportional gain used in control loop (default value is 2.0)
Outputs:
None
Notes:
- Actuators and gripper initial position are retrieved to set initial positions
- Actuator and gripper cyclic command objects are created in constructor. Their
references are used to update position and speed.
"""
self.proportional_gain = proportional_gain
###########################################################################################
# UDP and TCP sessions are used in this example.
# TCP is used to perform the change of servoing mode
# UDP is used for cyclic commands.
#
# 2 sessions have to be created: 1 for TCP and 1 for UDP
###########################################################################################
self.router = router
self.router_real_time = router_real_time
# Create base client using TCP router
self.base = BaseClient(self.router)
# Create base cyclic client using UDP router.
self.base_cyclic = BaseCyclicClient(self.router_real_time)
# Create base cyclic command object.
self.base_command = BaseCyclic_pb2.Command()
self.base_command.frame_id = 0
self.base_command.interconnect.command_id.identifier = 0
self.base_command.interconnect.gripper_command.command_id.identifier = 0
# Add motor command to interconnect's cyclic
self.motorcmd = self.base_command.interconnect.gripper_command.motor_cmd.add(
)
# Set gripper's initial position velocity and force
base_feedback = self.base_cyclic.RefreshFeedback()
self.motorcmd.position = base_feedback.interconnect.gripper_feedback.motor[
0].position
self.motorcmd.velocity = 0
self.motorcmd.force = 100
for actuator in base_feedback.actuators:
self.actuator_command = self.base_command.actuators.add()
self.actuator_command.position = actuator.position
self.actuator_command.velocity = 0.0
self.actuator_command.torque_joint = 0.0
self.actuator_command.command_id = 0
print("Position = ", actuator.position)
# Save servoing mode before changing it
self.previous_servoing_mode = self.base.GetServoingMode()
# Set base in low level servoing mode
servoing_mode_info = Base_pb2.ServoingModeInformation()
servoing_mode_info.servoing_mode = Base_pb2.LOW_LEVEL_SERVOING
self.base.SetServoingMode(servoing_mode_info)