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