def example_manipulation_protobuf_list():
# In Google Protocol Buffer, 'repeated' is used to designate a list of indeterminate length. Once affected to a Python
# variable they can be used in the same way as a standard list.
# Note that a repeated message field doesn't have an append() function, it has an
# add() function that create the new message object.
# For this example we will use the following two messages
# message Sequence {
# SequenceHandle handle = 1
# string name = 2
# string application_data = 3
# repeated SequenceTask tasks = 4
# }
# message SequenceTask {
# uint32 group_identifier = 1;
# Action action = 2;
# string application_data = 3;
# }
# For a clearer example the attribute action in SequenceTask message will be kept to default value
# Here's two ways to add to a repeated message field
# Create the parent message
sequence = Base_pb2.Sequence()
sequence.name = "Sequence"
# The 'extend' way
sequence_task_1 = Base_pb2.SequenceTask()
sequence_task_1.group_identifier = 10
action = sequence_task_1.action
action = Base_pb2.Action() # Using Action default constructor
sequence.tasks.extend([sequence_task_1]) # Extend expect an iterable
# Created for the add() function unique to repeated message field
sequence_task_2 = sequence.tasks.add()
sequence_task_2.group_identifier = 20
action = sequence_task_2.action
action = Base_pb2.Action() # Using Action default constructor
# Since sequence.task is a list we can use all the Python features to
# loop, iterate, interogate and print elements in that list.
for i in range(len(sequence.tasks)):
print("Sequence ID with index : {0}".format(
sequence.tasks[i].group_identifier))
# Lists have the iterator property of a Python list, so you can directly iterate
# throught element without creating a iterator as in the previous example
for task in sequence.tasks:
print("Sequence ID with object iterator : {0}".format(
task.group_identifier))
def example_angular_action_movement(base):
print("Starting angular action movement ...")
action = Base_pb2.Action()
action.name = "Example angular action movement"
action.application_data = ""
actuator_count = base.GetActuatorCount()
# Place arm straight up
for joint_id in range(actuator_count.count):
joint_angle = action.reach_joint_angles.joint_angles.joint_angles.add()
joint_angle.joint_identifier = joint_id
joint_angle.value = 0
e = threading.Event()
notification_handle = base.OnNotificationActionTopic(
check_for_end_or_abort(e), Base_pb2.NotificationOptions())
print("Executing action")
base.ExecuteAction(action)
print("Waiting for movement to finish ...")
finished = e.wait(TIMEOUT_DURATION)
base.Unsubscribe(notification_handle)
if finished:
print("Angular movement completed")
else:
print("Timeout on action notification wait")
return finished
def move_in_front_of_protection_zone(base):
print("Starting Cartesian action movement ...")
action = Base_pb2.Action()
action.name = "Example Cartesian action movement"
action.application_data = ""
cartesian_pose = action.reach_pose.target_pose
cartesian_pose.x = PROTECTION_ZONE_POS[0] - 0.1 # (meters)
cartesian_pose.y = PROTECTION_ZONE_POS[1] # (meters)
cartesian_pose.z = PROTECTION_ZONE_POS[2] # (meters)
cartesian_pose.theta_x = PROTECTION_ZONE_MOVEMENT_THETAS[0] # (degrees)
cartesian_pose.theta_y = PROTECTION_ZONE_MOVEMENT_THETAS[1] # (degrees)
cartesian_pose.theta_z = PROTECTION_ZONE_MOVEMENT_THETAS[2] # (degrees)
e = threading.Event()
notification_handle = base.OnNotificationActionTopic(
check_for_end_or_abort(e),
Base_pb2.NotificationOptions()
)
print("Executing action")
base.ExecuteAction(action)
print("Waiting for movement to finish ...")
e.wait()
print("Cartesian movement completed")
base.Unsubscribe(notification_handle)
def example_cartesian_action_movement(base, base_cyclic):
print("Starting Cartesian action movement ...")
action = Base_pb2.Action()
action.name = "Example Cartesian action movement"
action.application_data = ""
feedback = base_cyclic.RefreshFeedback()
cartesian_pose = action.reach_pose.target_pose
cartesian_pose.x = feedback.base.tool_pose_x # (meters)
cartesian_pose.y = feedback.base.tool_pose_y - 0.1 # (meters)
cartesian_pose.z = feedback.base.tool_pose_z - 0.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)
e = threading.Event()
notification_handle = base.OnNotificationActionTopic(
check_for_end_or_abort(e), Base_pb2.NotificationOptions())
print("Executing action")
base.ExecuteAction(action)
print("Waiting for movement to finish ...")
finished = e.wait(TIMEOUT_DURATION)
base.Unsubscribe(notification_handle)
if finished:
print("Cartesian movement completed")
else:
print("Timeout on action notification wait")
return finished
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 angular_action_movement(e0,base,base_cyclic,joint_positions=[0,0,0,0,0,0,0],speed=5,watch_for_flag=False,success_flag_shared=multiprocessing.Value("i",0)):
#At first change the joint speed to the desired value
#convert desired speed to time depending on the current Position
movement_time=convert_speed2time(target_pose=joint_positions,speed=speed,base_cyclic=base_cyclic)
action = Base_pb2.Action()
action.name = "angular action movement"
action.application_data = ""
#joint_speed=action.change_joint_speeds
actuator_count = base.GetActuatorCount()
#set speed this method is not working(?)
angular_speed=action.reach_joint_angles.constraint
angular_speed.type= 1 # type:speed 1=time in sec 2= deg/sec (asynchronous movements only)
#https://github.com/Kinovarobotics/kortex/blob/master/api_python/doc/markdown/messages/Base/JointTrajectoryConstraint.md#
angular_speed.value= movement_time+1 #time or deg/second
# Move arm to desired angles
i0=0
for joint_id in range(7):
joint_angle = action.reach_joint_angles.joint_angles.joint_angles.add()
joint_angle.joint_identifier = joint_id
joint_angle.value = joint_positions[i0]
i0+=1
e = threading.Event()
notification_handle = base.OnNotificationActionTopic(
check_for_end_or_abort(e),
Base_pb2.NotificationOptions()
)
print("Executing action")
base.ExecuteAction(action)
if watch_for_flag==True:
while success_flag_shared.value==0 or e.is_set()==False:
time.sleep(0.1)
finished=True
base.stop()
base.Unsubscribe(notification_handle)
else:
t0=time.time()
while e.is_set()==False and time.time()-t0<TIMEOUT_DURATION:
if e0.is_set()==True: #all processes were terminated from GUI
base.Stop()
base.Unsubscribe(notification_handle)
return False
finished = e.wait(TIMEOUT_DURATION) #waits until True
base.Unsubscribe(notification_handle)
if finished:
print("Angular movement completed")
else:
print("Timeout on action notification wait")
time.sleep(0.2) #damping time
return finished
def create_angular_action(actuator_count):
print("Creating angular action")
action = Base_pb2.Action()
action.name = "Example angular action"
action.application_data = ""
for joint_id in range(actuator_count):
joint_angle = action.reach_joint_angles.joint_angles.joint_angles.add()
joint_angle.value = 0.0
return action
def create_angular_action():
print("Creating angular action")
action = Base_pb2.Action()
action.name = "Example angular action"
action.application_data = ""
angle_value = [0, 0, 0, 0, 0, 0, 0]
for joint_id in range(7):
joint_angle = action.reach_joint_angles.joint_angles.joint_angles.add()
joint_angle.value = angle_value[joint_id]
return action
def move_to_waypoint_linear(e0,base, base_cyclic,waypoint,speed=0.1,watch_for_flag=False):
#Service Dok: https://github.com/Kinovarobotics/kortex/blob/master/api_python/doc/markdown/index.md
print("Moving to Waypoint ...")
#speed=Base_pb2.CartesianSpeed()
action = Base_pb2.Action()
action.name = "Example Cartesian action movement"
action.application_data = ""
#speed.translation = 0.01 #meters per second
#feedback = base_cyclic.RefreshFeedback()
cartesian_pose = action.reach_pose.target_pose
cartesian_speed= action.reach_pose.constraint
cartesian_speed.speed.translation = speed #meters/second
cartesian_speed.speed.orientation = 5 #deg/second
cartesian_pose.x = waypoint[0] # (meters)
cartesian_pose.y = waypoint[1] # (meters)
cartesian_pose.z = waypoint[2] # (meters)
cartesian_pose.theta_x = waypoint[3] # (degrees)
cartesian_pose.theta_y = waypoint[4] # (degrees)
cartesian_pose.theta_z = waypoint[5] # (degrees)
e = threading.Event()
notification_handle = base.OnNotificationActionTopic(
check_for_end_or_abort(e),
Base_pb2.NotificationOptions()
)
print("Executing action")
base.ExecuteAction(action)
if watch_for_flag==True:
while success_flag_shared.value==0 or e.is_set()==False:
time.sleep(0.1)
finished=True
base.stop()
base.Unsubscribe(notification_handle)
else:
t0=time.time()
while e.is_set()==False and time.time()-t0<TIMEOUT_DURATION:
if e0.is_set()==True: #all processes were terminated from GUI
base.Stop()
base.Unsubscribe(notification_handle)
return False
finished = e.wait(TIMEOUT_DURATION) #waits until True
base.Unsubscribe(notification_handle)
if finished:
print("Cartesian movement completed")
else:
print("Timeout on action notification wait")
return finished
def create_cartesian_action():
print("Creating Cartesian action")
action = Base_pb2.Action()
action.name = "Example Cartesian action"
action.application_data = ""
cartesian_pose = action.reach_pose.target_pose
cartesian_pose.x = 0.80 # (meters)
cartesian_pose.y = 0 # (meters)
cartesian_pose.z = 0.36 # (meters)
cartesian_pose.theta_x = 10 # (degrees)
cartesian_pose.theta_y = 90 # (degrees)
cartesian_pose.theta_z = 10 # (degrees)
return action
def create_cartesian_action(base_cyclic):
print("Creating Cartesian action")
action = Base_pb2.Action()
action.name = "Example Cartesian action"
action.application_data = ""
feedback = base_cyclic.RefreshFeedback()
cartesian_pose = action.reach_pose.target_pose
cartesian_pose.x = feedback.base.tool_pose_x # (meters)
cartesian_pose.y = feedback.base.tool_pose_y - 0.1 # (meters)
cartesian_pose.z = feedback.base.tool_pose_z - 0.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)
return action
def cartesian_action_movement(base, target_pos):
action = Base_pb2.Action()
action.name = "Example Cartesian action movement"
action.application_data = ""
cartesian_pose = action.reach_pose.target_pose
cartesian_pose.x = target_pos[0] # (meters)
cartesian_pose.y = target_pos[1] # (meters)
cartesian_pose.z = target_pos[2] # (meters)
cartesian_pose.theta_x = 180 # (degrees)
cartesian_pose.theta_y = 0 # (degrees)
cartesian_pose.theta_z = 90 # (degrees)
base.ExecuteAction(action)
print("Waiting 5 seconds for movement to finish ...")
time.sleep(5)
def example_cartesian_action_movement(base):
print("Starting Cartesian action movement ...")
action = Base_pb2.Action()
action.name = "Example Cartesian action movement"
action.application_data = ""
cartesian_pose = action.reach_pose.target_pose
cartesian_pose.x = 0.80 # (meters)
cartesian_pose.y = 0 # (meters)
cartesian_pose.z = 0.36 # (meters)
cartesian_pose.theta_x = 10 # (degrees)
cartesian_pose.theta_y = 90 # (degrees)
cartesian_pose.theta_z = 10 # (degrees)
print("Executing action")
base.ExecuteAction(action)
print("Waiting 20 seconds for movement to finish ...")
time.sleep(20)
print("Cartesian movement completed")
def example_angular_action_movement(base):
print("Starting angular action movement ...")
action = Base_pb2.Action()
action.name = "Example angular action movement"
action.application_data = ""
actuator_count = base.GetActuatorCount()
# Place arm straight up
for joint_id in range(actuator_count.count):
joint_angle = action.reach_joint_angles.joint_angles.joint_angles.add()
joint_angle.joint_identifier = joint_id
joint_angle.value = 0
print("Executing action")
base.ExecuteAction(action)
print("Waiting 20 seconds for movement to finish ...")
time.sleep(20)
print("Angular movement completed")
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 example_manipulation_protobuf_helpers():
# Google offers some helpers with Google Protocol Buffer,
# some of which are covered in this section.
# All the module google.protobuf documentation is available here:
# https://developers.google.com/protocol-buffers/docs/reference/python/
# First, the function include with message instance. We will cover the next function
# Clear
# MergeFrom
# CopyFrom
# The Clear function is straightforward - it clears all the message attributes.
# MergeFrom and CopyFrom have the same purpose: to duplicate data into another object.
# The difference between them is that CopyFrom will do a Clear before a MergeFrom.
# For its part MergeFrom will merge data if the new field is not empty.
# In the case of repeated, the content received in a parameter will be appended.
# For this example, we'll used the SSID message
# message Ssid {
# string identifier = 1;
# }
# First we'll create four of them, each with a unique string
ssid_1 = Base_pb2.Ssid()
ssid_1.identifier = ""
ssid_2 = Base_pb2.Ssid()
ssid_2.identifier = "123"
ssid_3 = Base_pb2.Ssid()
ssid_3.identifier = "@#$"
# Now merge ssid_1 into ssid_2 and print the identifier of ssid_2
ssid_2.MergeFrom(ssid_1)
print("Content ssid_2: {0}".format(ssid_2.identifier))
# output : Content ssid_2: 123
# Now copy ssid_1 into ssid_3 and print the identifier of ssid_3
ssid_3.CopyFrom(ssid_1)
print("Content ssid_3: {0}".format(ssid_3.identifier))
# output : Content ssid_3:
# For more functions consult the Class Message documentation
# https://developers.google.com/protocol-buffers/docs/reference/python/google.protobuf.message.Message-class
# From the Google Protocol Buffer library you can use the json_format module.
# One useful function is the MessageToJson.
# This function serializes the Google Protocol Buffer message to a JSON object. It is helpful when you want to print a large object into
# a human-readable format.
# Here's an example with the following two messages
#
# message Sequence {
# SequenceHandle handle = 1
# string name = 2
# string application_data = 3
# repeated SequenceTask tasks = 4
# }
# message SequenceTask {
# uint32 group_identifier = 1;
# Action action = 2;
# string application_data = 3;
# }
# populate the message
sequence = Base_pb2.Sequence()
sequence.name = "A Name"
for i in range(5):
sequence_task = sequence.tasks.add()
sequence_task.group_identifier = 10 * (
i + 1) # Some further function doesn't print if value = 0
action = sequence_task.action
action = Base_pb2.Action() # Using Action default constructor
# need to import the module
from google.protobuf import json_format
# now get the JSON object
json_object = json_format.MessageToJson(sequence)
# now print it
print("Json object:")
print(json_object)
# output:
# Json object
# {
# "name": "A Name",
# "tasks": [
# {
# "groupIdentifier": 10
# },
# {
# "groupIdentifier": 20
# },
# {
# "groupIdentifier": 30
# },
# {
# "groupIdentifier": 40
# },
# {
# "groupIdentifier": 50
# }
# ]
# }
# From the Google Protocol Buffer library you can used the text_format module.
# A useful function is the MessageToString. It has the same purpose as
# MessageToJson - convert the message to a human readable format.
# For the example we'll reuse the sequence object created in the previous example
# First import the module
from google.protobuf import text_format
# Now print
print("Text format:")
print(text_format.MessageToString(sequence))
