def example_move_to_home_position(base):
# 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
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 = 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
e = threading.Event()
notification_handle = base.OnNotificationActionTopic(
check_for_end_or_abort(e),
Base_pb2.NotificationOptions()
)
base.ExecuteActionFromReference(action_handle)
finished = e.wait(TIMEOUT_DURATION)
base.Unsubscribe(notification_handle)
if finished:
print("Safe position reached")
else:
print("Timeout on action notification wait")
return finished
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 call_kinova_web_sequence(base, base_cyclic,seq_name=""):
#get the Sequence
sequences = base.ReadAllSequences()
sequence_handle = None
for sequence in sequences.sequence_list:
if sequence.name == seq_name:
#print(sequence)
sequence_handle=sequence.handle
print("Executing Sequence: %s"%(seq_name))
if sequence_handle == None:
print("Can't find sequence, check name.")
sys.exit(0)
e = threading.Event()
notification_handle = base.OnNotificationSequenceInfoTopic(
check_for_sequence_end_or_abort(e),
Base_pb2.NotificationOptions()
)
base.PlaySequence(sequence_handle)
#comes from here: https://github.com/Kinovarobotics/kortex/blob/master/api_cpp/doc/markdown/summary_pages/Base.md
print("Waiting for movement to finish ...")
finished = e.wait(TIMEOUT_DURATION)
base.Unsubscribe(notification_handle)
if not finished:
print("Timeout on action notification wait")
return finished
def example_create_sequence(base, base_cyclic):
print("Creating Action for Sequence")
actuator_count = base.GetActuatorCount().count
angular_action = create_angular_action(actuator_count)
cartesian_action = create_cartesian_action(base_cyclic)
print("Creating Sequence")
sequence = Base_pb2.Sequence()
sequence.name = "Example sequence"
print("Appending Actions to Sequence")
task_1 = sequence.tasks.add()
task_1.group_identifier = 0
task_1.action.CopyFrom(cartesian_action)
task_2 = sequence.tasks.add()
task_2.group_identifier = 1 # sequence elements with same group_id are played at the same time
task_2.action.CopyFrom(angular_action)
e = threading.Event()
notification_handle = base.OnNotificationSequenceInfoTopic(
check_for_sequence_end_or_abort(e), Base_pb2.NotificationOptions())
print("Creating sequence on device and executing it")
handle_sequence = base.CreateSequence(sequence)
base.PlaySequence(handle_sequence)
print("Waiting for movement to finish ...")
finished = e.wait(TIMEOUT_DURATION)
base.Unsubscribe(notification_handle)
if not finished:
print("Timeout on action notification wait")
return finished
def example_angular_trajectory_movement(base):
constrained_joint_angles = Base_pb2.ConstrainedJointAngles()
actuator_count = base.GetActuatorCount()
# Place arm straight up
for joint_id in range(actuator_count.count):
joint_angle = constrained_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("Reaching joint angles...")
base.PlayJointTrajectory(constrained_joint_angles)
print("Waiting for movement to finish ...")
finished = e.wait(TIMEOUT_DURATION)
base.Unsubscribe(notification_handle)
if finished:
print("Joint angles reached")
else:
print("Timeout on action notification wait")
return finished
def example_cartesian_trajectory_movement(base, base_cyclic):
constrained_pose = Base_pb2.ConstrainedPose()
feedback = base_cyclic.RefreshFeedback()
cartesian_pose = constrained_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("Reaching cartesian pose...")
base.PlayCartesianTrajectory(constrained_pose)
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 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 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_move_to_start_position(base):
# 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
base.SetServoingMode(base_servo_mode)
# Move arm to ready position
constrained_joint_angles = Base_pb2.ConstrainedJointAngles()
actuator_count = base.GetActuatorCount().count
angles = [0.0] * actuator_count
# Actuator 4 at 90 degrees
for joint_id in range(len(angles)):
joint_angle = constrained_joint_angles.joint_angles.joint_angles.add()
joint_angle.joint_identifier = joint_id
joint_angle.value = angles[joint_id]
e = threading.Event()
notification_handle = base.OnNotificationActionTopic(
check_for_end_or_abort(e), Base_pb2.NotificationOptions())
print("Reaching joint angles...")
base.PlayJointTrajectory(constrained_joint_angles)
print("Waiting for movement to finish ...")
finished = e.wait(TIMEOUT_DURATION)
base.Unsubscribe(notification_handle)
if finished:
print("Joint angles reached")
else:
print("Timeout on action notification wait")
return finished
def example_notification(base):
def notification_callback(data):
print("****************************")
print("* Callback function called *")
print(json_format.MessageToJson(data))
print("****************************")
# Subscribe to ConfigurationChange notifications
print("Subscribing to ConfigurationChange notifications")
try:
notif_handle = base.OnNotificationConfigurationChangeTopic(
notification_callback, Base_pb2.NotificationOptions())
except KException as k_ex:
print("Error occured: {}".format(k_ex))
except Exception:
print("Error occured")
# ... miscellaneous tasks
time.sleep(3)
# Create a user profile to trigger a notification
full_user_profile = Base_pb2.FullUserProfile()
full_user_profile.user_profile.username = '******'
full_user_profile.user_profile.firstname = 'Johnny'
full_user_profile.user_profile.lastname = 'Cash'
full_user_profile.user_profile.application_data = "Custom Application Stuff"
full_user_profile.password = "******"
user_profile_handle = Base_pb2.UserProfileHandle()
try:
print("Creating user profile to trigger notification")
user_profile_handle = base.CreateUserProfile(full_user_profile)
except KException:
print("User profile creation failed")
# Following the creation of the user profile, we should receive the ConfigurationChange notification (notification_callback() should be called)
print("User {0} created".format(full_user_profile.user_profile.username))
# Give time for the notification to arrive
time.sleep(3)
print("Now unsubscribing from ConfigurationChange notifications")
base.Unsubscribe(notif_handle)
try:
print("Deleting previously created user profile ({0})".format(
full_user_profile.user_profile.username))
base.DeleteUserProfile(
user_profile_handle
) # Should not have received notification about this modification
except KException:
print("User profile deletion failed")
# Sleep to confirm that ConfigurationChange notification is not raised anymore after the unsubscribe
time.sleep(3)
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 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 waypoint_trajectory_movement(base, base_cyclic,waypoint,speed=0.1,watch_for_flag=False):
constrained_pose = Base_pb2.ConstrainedPose()
constrained_pose.constraint.speed.translation = speed # m/s
constrained_pose.constraint.speed.orientation = 30 # deg/sec
cartesian_pose = constrained_pose.target_pose
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("Reaching cartesian pose...")
base.PlayCartesianTrajectory(constrained_pose)
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")
return finished
def example_trajectory(base, base_cyclic):
base_servo_mode = Base_pb2.ServoingModeInformation()
base_servo_mode.servoing_mode = Base_pb2.SINGLE_LEVEL_SERVOING
base.SetServoingMode(base_servo_mode)
jointPoses = tuple(tuple())
product = base.GetProductConfiguration()
if (product.model == Base_pb2.ProductConfiguration__pb2.MODEL_ID_L53 or
product.model == Base_pb2.ProductConfiguration__pb2.MODEL_ID_L31):
if (product.model == Base_pb2.ProductConfiguration__pb2.MODEL_ID_L31):
jointPoses = ((0.0, 344.0, 75.0, 360.0, 300.0,
0.0), (0.0, 21.0, 145.0, 272.0, 32.0, 273.0),
(42.0, 334.0, 79.0, 241.0, 305.0, 56.0))
else:
# Binded to degrees of movement and each degrees correspond to one degree of liberty
degreesOfFreedom = base.GetActuatorCount()
if (degreesOfFreedom.count == 6):
jointPoses = ((360.0, 35.6, 281.8, 0.8, 23.8,
88.9), (359.6, 49.1, 272.1, 0.3, 47.0, 89.1),
(320.5, 76.5, 335.5, 293.4, 46.1, 165.6),
(335.6, 38.8, 266.1, 323.9, 49.7,
117.3), (320.4, 76.5, 335.5, 293.4, 46.1,
165.6), (28.8, 36.7, 273.2, 40.8, 39.5,
59.8), (360.0, 45.6, 251.9,
352.2, 54.3, 101.0))
else:
jointPoses = ((360.0, 35.6, 180.7, 281.8, 0.8, 23.8, 88.9),
(359.6, 49.1, 181.0, 272.1, 0.3, 47.0,
89.1), (320.5, 76.5, 166.5, 335.5, 293.4, 46.1,
165.6), (335.6, 38.8, 177.0, 266.1,
323.9, 49.7, 117.3),
(320.4, 76.5, 166.5, 335.5, 293.4, 46.1,
165.6), (28.8, 36.7, 174.7, 273.2, 40.8, 39.5,
59.8), (360.0, 45.6, 171.0, 251.9,
352.2, 54.3, 101.0))
else:
print(
"Product is not compatible to run this example please contact support with KIN number bellow"
)
print("Product KIN is : " + product.kin())
waypoints = Base_pb2.WaypointList()
waypoints.duration = 0.0
waypoints.use_optimal_blending = False
index = 0
for jointPose in jointPoses:
waypoint = waypoints.waypoints.add()
waypoint.name = "waypoint_" + str(index)
durationFactor = 1
# Joints/motors 5 and 7 are slower and need more time
if (index == 4 or index == 6):
durationFactor = 6 # Min 30 seconds
waypoint.angular_waypoint.CopyFrom(
populateAngularPose(jointPose, durationFactor))
index = index + 1
# Verify validity of waypoints
result = base.ValidateWaypointList(waypoints)
if (len(result.trajectory_error_report.trajectory_error_elements) == 0):
e = threading.Event()
notification_handle = base.OnNotificationActionTopic(
check_for_end_or_abort(e), Base_pb2.NotificationOptions())
print("Reaching angular pose trajectory...")
base.ExecuteWaypointTrajectory(waypoints)
print("Waiting for trajectory 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
else:
print("Error found in trajectory")
print(result.trajectory_error_report)
return finished
def example_trajectory(base, base_cyclic):
base_servo_mode = Base_pb2.ServoingModeInformation()
base_servo_mode.servoing_mode = Base_pb2.SINGLE_LEVEL_SERVOING
base.SetServoingMode(base_servo_mode)
product = base.GetProductConfiguration()
waypointsDefinition = tuple(tuple())
if (product.model == Base_pb2.ProductConfiguration__pb2.MODEL_ID_L53 or
product.model == Base_pb2.ProductConfiguration__pb2.MODEL_ID_L31):
if (product.model == Base_pb2.ProductConfiguration__pb2.MODEL_ID_L31):
kTheta_x = 90.6
kTheta_y = -1.0
kTheta_z = 150.0
waypointsDefinition = ((0.439, 0.194, 0.448, 0.0, kTheta_x,
kTheta_y, kTheta_z),
(0.200, 0.150, 0.400, 0.0, kTheta_x,
kTheta_y, kTheta_z), (0.350, 0.050, 0.300,
0.0, kTheta_x,
kTheta_y, kTheta_z))
else:
kTheta_x = 90.0
kTheta_y = 0.0
kTheta_z = 90.0
waypointsDefinition = ((0.7, 0.0, 0.5, 0.0, kTheta_x, kTheta_y,
kTheta_z), (0.7, 0.0, 0.33, 0.1, kTheta_x,
kTheta_y, kTheta_z),
(0.7, 0.48, 0.33, 0.1, kTheta_x, kTheta_y,
kTheta_z), (0.61, 0.22, 0.4, 0.1, kTheta_x,
kTheta_y, kTheta_z),
(0.7, 0.48, 0.33, 0.1, kTheta_x, kTheta_y,
kTheta_z), (0.63, -0.22, 0.45, 0.1,
kTheta_x, kTheta_y, kTheta_z),
(0.65, 0.05, 0.33, 0.0, kTheta_x, kTheta_y,
kTheta_z))
else:
print(
"Product is not compatible to run this example please contact support with KIN number bellow"
)
print("Product KIN is : " + product.kin())
waypoints = Base_pb2.WaypointList()
waypoints.duration = 0.0
waypoints.use_optimal_blending = False
index = 0
for waypointDefinition in waypointsDefinition:
waypoint = waypoints.waypoints.add()
waypoint.name = "waypoint_" + str(index)
waypoint.cartesian_waypoint.CopyFrom(
populateCartesianCoordinate(waypointDefinition))
index = index + 1
# Verify validity of waypoints
result = base.ValidateWaypointList(waypoints)
if (len(result.trajectory_error_report.trajectory_error_elements) == 0):
e = threading.Event()
notification_handle = base.OnNotificationActionTopic(
check_for_end_or_abort(e), Base_pb2.NotificationOptions())
print("Moving cartesian trajectory...")
base.ExecuteWaypointTrajectory(waypoints)
print("Waiting for trajectory to finish ...")
finished = e.wait(TIMEOUT_DURATION)
base.Unsubscribe(notification_handle)
if finished:
print("Cartesian trajectory with no optimization completed ")
e_opt = threading.Event()
notification_handle_opt = base.OnNotificationActionTopic(
check_for_end_or_abort(e_opt), Base_pb2.NotificationOptions())
waypoints.use_optimal_blending = True
base.ExecuteWaypointTrajectory(waypoints)
print("Waiting for trajectory to finish ...")
finished_opt = e_opt.wait(TIMEOUT_DURATION)
base.Unsubscribe(notification_handle_opt)
if (finished_opt):
print("Cartesian trajectory with optimization completed ")
else:
print(
"Timeout on action notification wait for optimized trajectory"
)
return finished_opt
else:
print(
"Timeout on action notification wait for non-optimized trajectory"
)
return finished
else:
print("Error found in trajectory")
result.trajectory_error_report.PrintDebugString()