class KinovaController:
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 set_single_level_servoing_mode(self):
# Save servoing mode before changing it
self.previous_servoing_mode = self.base.GetServoingMode()
# Set base in single level servoing mode
servoing_mode_info = Base_pb2.ServoingModeInformation()
# print("Setting single level servoi
print("Current Mode:")
print(servoing_mode_info)
servoing_mode_info.servoing_mode = Base_pb2.SINGLE_LEVEL_SERVOING
self.base.SetServoingMode(servoing_mode_info)
print("Waiting 5 second for changing to single level servoing mode...")
time.sleep(5)
print("After setting single level")
print(servoing_mode_info)
def move_arm(self, pos_list):
self.set_single_level_servoing_mode()
# for count, joint_angle in enumerate(self.joint_angle_list):
for count, pos in enumerate(pos_list):
print("Moving to ", pos)
cartesian_action_movement(self.base, pos)
self.base_feedback = self.SendCallWithRetry(
self.base_cyclic.RefreshFeedback, 3)
joint_angles = [
self.base_feedback.actuators[i].position for i in range(7)
]
@staticmethod
def SendCallWithRetry(call, retry, *args):
i = 0
arg_out = []
while i < retry:
try:
arg_out = call(*args)
break
except:
i = i + 1
continue
if i == retry:
print("Failed to communicate")
return arg_out
class TorqueExample:
def __init__(self, router, router_real_time):
self.expected_number_of_actuators = 7 # example works for 7dof Gen3
# self.torque_amplification = 2.0 # Torque measure on 7th actuator is sent as a command to first actuator
self.torque_amplification = 20.0 # Torque measure on 7th 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 = 0
# 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()
self.actuator_count += 1
# 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 MoveToHomePosition(self):
# Make sure the arm is in Single Level Servoing mode
base_servo_mode = Base_pb2.ServoingModeInformation()
base_servo_mode.servoing_mode = Base_pb2.SINGLE_LEVEL_SERVOING
self.base.SetServoingMode(base_servo_mode)
# Move arm to ready position
print("Moving the arm to a safe position")
action_type = Base_pb2.RequestedActionType()
action_type.action_type = Base_pb2.REACH_JOINT_ANGLES
action_list = self.base.ReadAllActions(action_type)
action_handle = None
for action in action_list.action_list:
if action.name == "Home":
action_handle = action.handle
if action_handle == None:
print("Can't reach safe position. Exiting")
return False
self.base.ExecuteActionFromReference(action_handle)
time.sleep(20) # Leave time to action to complete
return True
def InitCyclic(self, sampling_time_cyclic, t_end, print_stats):
if self.cyclic_running:
return True
# Move to Home position first
if not self.MoveToHomePosition():
return False
print("Init Cyclic")
sys.stdout.flush()
base_feedback = self.SendCallWithRetry(self.base_cyclic.RefreshFeedback, 3)
if base_feedback:
self.base_feedback = base_feedback
if len(self.base_feedback.actuators) == self.expected_number_of_actuators:
# Init command frame
for x in range(self.actuator_count):
self.base_command.actuators[x].flags = 1 # servoing
self.base_command.actuators[x].position = self.base_feedback.actuators[x].position
# First actuator is going to be controlled in torque
# To ensure continuity, torque command is set to measure
self.base_command.actuators[0].torque_joint = self.base_feedback.actuators[0].torque
# Set arm in LOW_LEVEL_SERVOING
base_servo_mode = Base_pb2.ServoingModeInformation()
base_servo_mode.servoing_mode = Base_pb2.LOW_LEVEL_SERVOING
self.base.SetServoingMode(base_servo_mode)
# Send first frame
self.base_feedback = self.base_cyclic.Refresh(self.base_command, 0, self.sendOption)
# Set first actuator in torque mode now that the command is equal to measure
control_mode_message = ActuatorConfig_pb2.ControlModeInformation()
control_mode_message.control_mode = ActuatorConfig_pb2.ControlMode.Value('TORQUE')
device_id = 1 # first actuator as id = 1
self.SendCallWithRetry(self.actuator_config.SetControlMode, 3, control_mode_message, device_id)
# Init cyclic thread
self.cyclic_t_end = t_end
self.cyclic_thread = threading.Thread(target=self.RunCyclic, args=(sampling_time_cyclic, print_stats))
self.cyclic_thread.daemon = True
self.cyclic_thread.start()
return True
else:
print("InitCyclic: number of actuators in base_feedback does not match expected number")
return False
else:
print("InitCyclic: failed to communicate")
return False
def RunCyclic(self, t_sample, print_stats):
self.cyclic_running = True
print("Run Cyclic")
sys.stdout.flush()
cyclic_count = 0 # Counts refresh
stats_count = 0 # Counts stats prints
failed_cyclic_count = 0 # Count communication timeouts
# Initial delta between first and last actuator
init_delta_position = self.base_feedback.actuators[0].position - self.base_feedback.actuators[6].position
# Initial first and last actuator torques; avoids unexpected movement due to torque offsets
init_last_torque = self.base_feedback.actuators[6].torque
init_first_torque = -self.base_feedback.actuators[0].torque # Torque measure is reversed compared to actuator direction
t_now = time.time()
t_cyclic = t_now # cyclic time
t_stats = t_now # print time
t_init = t_now # init time
print("Running torque control example for {} seconds".format(self.cyclic_t_end))
while not self.kill_the_thread:
t_now = time.time()
# Cyclic Refresh
if (t_now - t_cyclic) >= t_sample:
t_cyclic = t_now
# Position command to first actuator is set to measured one to avoid following error to trigger
# Bonus: When doing this instead of disabling the following error, if communication is lost and first
# actuator continue to move under torque command, resulting position error with command will
# trigger a following error and switch back the actuator in position command to hold its position
self.base_command.actuators[0].position = self.base_feedback.actuators[0].position
# First actuator torque command is set to last actuator torque measure times an amplification
# self.base_command.actuators[0].torque_joint = init_first_torque + \
# self.torque_amplification * (self.base_feedback.actuators[6].torque - init_last_torque)
self.base_command.actuators[0].torque_joint = 5.0
print(self.base_command.actuators[0].torque_joint)
# First actuator position is sent as a command to last actuator
self.base_command.actuators[6].position = self.base_feedback.actuators[0].position - init_delta_position
# Incrementing identifier ensure actuators can reject out of time frames
self.base_command.frame_id += 1
if self.base_command.frame_id > 65535:
self.base_command.frame_id = 0
for i in range(self.expected_number_of_actuators):
self.base_command.actuators[i].command_id = self.base_command.frame_id
# Frame is sent
try:
self.base_feedback = self.base_cyclic.Refresh(self.base_command, 0, self.sendOption)
except:
failed_cyclic_count = failed_cyclic_count + 1
print("failed")
cyclic_count = cyclic_count + 1
# Stats Print
if print_stats and ((t_now - t_stats) > 1):
t_stats = t_now
stats_count = stats_count + 1
cyclic_count = 0
failed_cyclic_count = 0
sys.stdout.flush()
if self.cyclic_t_end != 0 and (t_now - t_init > self.cyclic_t_end):
print("Cyclic Finished")
sys.stdout.flush()
break
self.cyclic_running = False
return True
def StopCyclic(self):
print ("Stopping the cyclic and putting the arm back in position mode...")
if self.already_stopped:
return
# Kill the thread first
if self.cyclic_running:
self.kill_the_thread = True
self.cyclic_thread.join()
# Set first actuator back in position mode
control_mode_message = ActuatorConfig_pb2.ControlModeInformation()
control_mode_message.control_mode = ActuatorConfig_pb2.ControlMode.Value('POSITION')
device_id = 1 # first actuator has id = 1
self.SendCallWithRetry(self.actuator_config.SetControlMode, 3, control_mode_message, device_id)
base_servo_mode = Base_pb2.ServoingModeInformation()
base_servo_mode.servoing_mode = Base_pb2.SINGLE_LEVEL_SERVOING
self.base.SetServoingMode(base_servo_mode)
self.cyclic_t_end = 0.1
self.already_stopped = True
print('Clean Exit')
@staticmethod
def SendCallWithRetry(call, retry, *args):
i = 0
arg_out = []
while i < retry:
try:
arg_out = call(*args)
break
except:
i = i + 1
continue
if i == retry:
print("Failed to communicate")
return arg_out
class GripperLowLevelExample:
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)
def Cleanup(self):
"""
Restore arm's servoing mode to the one that
was effective before running the example.
Inputs:
None
Outputs:
None
Notes:
None
"""
# Restore servoing mode to the one that was in use before running the example
self.base.SetServoingMode(self.previous_servoing_mode)
def Goto(self, target_position):
"""
Position gripper to a requested target position using a simple
proportional feedback loop which changes speed according to error
between target position and current gripper position
Inputs:
float target_position: position (0% - 100%) to send gripper to.
Outputs:
Returns True if gripper was positionned successfully, returns False
otherwise.
Notes:
- This function blocks until position is reached.
- If target position exceeds 100.0, its value is changed to 100.0.
- If target position is below 0.0, its value is set to 0.0.
"""
if target_position > 100.0:
target_position = 100.0
if target_position < 0.0:
target_position = 0.0
while True:
try:
base_feedback = self.base_cyclic.Refresh(self.base_command)
# Calculate speed according to position error (target position VS current position)
position_error = target_position - base_feedback.interconnect.gripper_feedback.motor[
0].position
# If positional error is small, stop gripper
if abs(position_error) < 1.5:
position_error = 0
self.motorcmd.velocity = 0
self.base_cyclic.Refresh(self.base_command)
return True
else:
self.motorcmd.velocity = self.proportional_gain * abs(
position_error)
if self.motorcmd.velocity > 100.0:
self.motorcmd.velocity = 100.0
self.motorcmd.position = 100.0
if position_error < 0.0:
self.motorcmd.position = 0.0
except Exception as e:
print("Error in refresh: " + str(e))
return False
time.sleep(0.001)
return True
class KinovaRobotEnv:
metadata = {'render_modes':['human', 'rbg_array']}
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 seed(self, seed):
pass
def close(self):
self.connection.__exit__(None, None, None)
def render(self, mode='human', width=84, height=84):
# return np.zeros((width, height, 3))
ret, frame = self.webcam.read()
frame = cv2.cvtColor(frame, cv2.COLOR_BGR2RGB)
if mode == 'human':
cv2.imshow('', frame)
elif mode == 'rgb_array':
frame = cv2.resize(frame, (width, height))
return frame
def sample_goal(self, target_range=None):
if target_range is None:
target_range = self.target_range
self.goal = self.init_xyz + (-1 + 2*np.random.rand(*self.init_xyz.shape))*self.target_range
return self.goal
def reset(self):
self.sample_goal()
self._set_to_home()
return self._get_obs_dict()
def step(self, act):
print(f'act: {act}')
act = act.copy()*self.action_scale
action = Base_pb2.Action()
action.name = "Example Cartesian action movement"
action.application_data = ""
action.reach_pose.constraint.speed.translation = self.speed
feedback = self.base_cyclic.RefreshFeedback()
cartesian_pose = action.reach_pose.target_pose
cartesian_pose.x = feedback.base.tool_pose_x + act[0] # (meters)
cartesian_pose.y = feedback.base.tool_pose_y + act[1] # (meters)
cartesian_pose.z = feedback.base.tool_pose_z + act[2] # (meters)
cartesian_pose.theta_x = feedback.base.tool_pose_theta_x # (degrees)
cartesian_pose.theta_y = feedback.base.tool_pose_theta_y # (degrees)
cartesian_pose.theta_z = feedback.base.tool_pose_theta_z # (degrees)
self.base.ExecuteAction(action)
duration = 1. + (np.linalg.norm(act)/self.speed)
print(f'step w duration {duration}')
time.sleep(duration)
obs_dict = self._get_obs_dict()
info = dict()
reward = self.compute_reward(
obs_dict['achieved_goal'],
obs_dict['desired_goal'],
info,
)
done = True if np.abs(reward) < self.distance_threshold else False
info['is_success'] = done
return obs_dict, reward, done, info
def _set_to(self, xyz):
action = Base_pb2.Action()
action.name = "Example Cartesian action movement"
action.application_data = ""
action.reach_pose.constraint.speed.translation = self.speed
feedback = self.base_cyclic.RefreshFeedback()
current_pos = np.asarray([feedback.base.tool_pose_x, feedback.base.tool_pose_y, feedback.base.tool_pose_z])
distance_to_xyz = np.linalg.norm(current_pos - xyz)
duration = 1. + (distance_to_xyz/self.speed)
print(f'_set_to w duration {duration}')
cartesian_pose = action.reach_pose.target_pose
cartesian_pose.x = xyz[0]
cartesian_pose.y = xyz[1]
cartesian_pose.z = xyz[2]
cartesian_pose.theta_x = feedback.base.tool_pose_theta_x # (degrees)
cartesian_pose.theta_y = feedback.base.tool_pose_theta_y # (degrees)
cartesian_pose.theta_z = feedback.base.tool_pose_theta_z # (degrees)
self.base.ExecuteAction(action)
time.sleep(duration)
def _set_to_home(self):
# Make sure the arm is in Single Level Servoing mode
base_servo_mode = Base_pb2.ServoingModeInformation()
base_servo_mode.servoing_mode = Base_pb2.SINGLE_LEVEL_SERVOING
self.base.SetServoingMode(base_servo_mode)
# Move arm to ready position
print("Moving the arm to a safe position")
action_type = Base_pb2.RequestedActionType()
action_type.action_type = Base_pb2.REACH_JOINT_ANGLES
action_list = self.base.ReadAllActions(action_type)
action_handle = None
for action in action_list.action_list:
if action.name == "Home":
action_handle = action.handle
if action_handle == None:
raise Run("Can't reach safe position. Exiting")
self.base.ExecuteActionFromReference(action_handle)
print('Resetting for duration 6')
time.sleep(6)
def compute_reward(self, achieved_goal, desired_goal, info):
d = np.linalg.norm(achieved_goal - desired_goal, axis=-1)
return 0. if d < self.distance_threshold else -1.
def _is_success(self, achieved_goal, desired_goal):
d = np.linalg.norm(achieved_goal - desired_goal, axis=-1)
return True if d < self.distance_threshold else False
def _get_obs(self):
feedback = self.base_cyclic.RefreshFeedback()
observation = np.asarray([
feedback.base.tool_pose_x,
feedback.base.tool_pose_y,
feedback.base.tool_pose_z,
])
return observation
def _get_obs_dict(self):
observation = self._get_obs()
achieved_goal = observation[:3]
desired_goal = self.goal
return dict(
observation=observation,
achieved_goal=achieved_goal,
desired_goal=desired_goal,
)