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,
)
