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()
# We suppose the arm is a 7DOF
angles = [0.0, 0.0, 0.0, 90.0, 0.0, 0.0, 0.0]
# 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]
print("Reaching joint angles...")
base.PlayJointTrajectory(constrained_joint_angles)
print("Waiting 20 seconds for movement to finish ...")
time.sleep(20)
print("Joint angles reached")
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_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_send_joint_speeds(base):
joint_speeds = Base_pb2.JointSpeeds()
speeds = [10.0, 0, -10.0, 0, 10.0, 0, -10.0]
i = 0
for speed in speeds:
joint_speed = joint_speeds.joint_speeds.add()
joint_speed.joint_identifier = i
joint_speed.value = speed
joint_speed.duration = 0
i = i + 1
print("Sending the joint speeds for 10 seconds...")
base.SendJointSpeedsCommand(joint_speeds)
time.sleep(10)
joint_speeds_stop = Base_pb2.JointSpeeds()
stop = [0, 0, 0, 0, 0, 0, 0]
i = 0
for speed in stop:
joint_speed = joint_speeds_stop.joint_speeds.add()
joint_speed.joint_identifier = i
joint_speed.value = speed
joint_speed.duration = 0
i = i + 1
print("Stopping the robot")
base.SendJointSpeedsCommand(joint_speeds_stop)
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 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_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_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 ExampleSendGripperCommands(self):
# Create the GripperCommand we will send
gripper_command = Base_pb2.GripperCommand()
finger = gripper_command.gripper.finger.add()
# Close the gripper with position increments
print("Performing gripper test in position...")
gripper_command.mode = Base_pb2.GRIPPER_POSITION
position = 0.00
finger.finger_identifier = 1
while position < 1.0:
finger.value = position
print("Going to position {:0.2f}...".format(finger.value))
self.base.SendGripperCommand(gripper_command)
position += 0.1
time.sleep(1)
# Set speed to open gripper
print("Opening gripper using speed command...")
gripper_command.mode = Base_pb2.GRIPPER_SPEED
finger.value = 0.1
self.base.SendGripperCommand(gripper_command)
gripper_request = Base_pb2.GripperRequest()
# Wait for reported position to be opened
gripper_request.mode = Base_pb2.GRIPPER_POSITION
while True:
gripper_measure = self.base.GetMeasuredGripperMovement(
gripper_request)
if len(gripper_measure.finger):
print("Current position is : {0}".format(
gripper_measure.finger[0].value))
if gripper_measure.finger[0].value < 0.01:
break
else: # Else, no finger present in answer, end loop
break
# Set speed to close gripper
print("Closing gripper using speed command...")
gripper_command.mode = Base_pb2.GRIPPER_SPEED
finger.value = -0.1
self.base.SendGripperCommand(gripper_command)
# Wait for reported speed to be 0
gripper_request.mode = Base_pb2.GRIPPER_SPEED
while True:
gripper_measure = self.base.GetMeasuredGripperMovement(
gripper_request)
if len(gripper_measure.finger):
print("Current speed is : {0}".format(
gripper_measure.finger[0].value))
if gripper_measure.finger[0].value == 0.0:
break
else: # Else, no finger present in answer, end loop
break
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 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 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 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_twist_command(base):
command = Base_pb2.TwistCommand()
command.reference_frame = Base_pb2.CARTESIAN_REFERENCE_FRAME_TOOL
command.duration = 0
twist = command.twist
twist.linear_x = 0
twist.linear_y = 0.03
twist.linear_z = 0
twist.angular_x = 0
twist.angular_y = 0
twist.angular_z = 5
print ("Sending the twist command for 5 seconds...")
base.SendTwistCommand(command)
# Let time for twist to be executed
time.sleep(5)
# Send all 0 to make it stop
twist.linear_x = 0
twist.linear_y = 0
twist.linear_z = 0
twist.angular_x = 0
twist.angular_y = 0
twist.angular_z = 0
print ("Stopping the robot...")
base.SendTwistCommand(command)
def example_create_sequence(base):
print("Creating Action for Sequence")
angular_action = create_angular_action()
cartesian_action = create_cartesian_action()
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(angular_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(cartesian_action)
print("Creating sequence on device and executing it")
handle_sequence = base.CreateSequence(sequence)
base.PlaySequence(handle_sequence)
print("Waiting 30 seconds for movement to finish ...")
time.sleep(30)
print("Sequence completed")
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')
def tool_twist_time(e,base,timer=3,pose_distance=[0,-0.2,0,0,0,0]):
#works like this: within 3 seconds move 20cm in negative y direction
#when this function is called, the robot moves unlimited into the
#pose_increment directions, where the valuas are the speed and direction(+/-)
#the movement can be stopped with base.Stop()
command = Base_pb2.TwistCommand()
command.reference_frame = Base_pb2.CARTESIAN_REFERENCE_FRAME_TOOL
command.duration = 0
twist = command.twist
twist.linear_x = pose_distance[0]/timer #meters/second
twist.linear_y = pose_distance[1]/timer #meters/second
twist.linear_z = pose_distance[2]/timer #meters/second
twist.angular_x = pose_distance[3]/timer #deg/second
twist.angular_y = pose_distance[4]/timer #deg/second
twist.angular_z = pose_distance[5]/timer #deg/second
base.SendTwistCommand(command)
# Let time for twist to be executed
#time.sleep(5)
t0=time.time()
while e.is_set()==False and time.time()-t0<timer:
time.sleep(0.05)
base.Stop()
#print ("Stopping the robot...")
#base.Stop()
#time.sleep(1)
return True
def example_twist_command(base,pose_increment=[0,0,0,0,0,0]):
#when this function is called, the robot moves unlimited into the
#pose_increment directions, where the valuas are the speed and direction(+/-)
#the movement can be stopped with base.Stop()
command = Base_pb2.TwistCommand()
command.reference_frame = Base_pb2.CARTESIAN_REFERENCE_FRAME_TOOL
command.duration = 0
twist = command.twist
twist.linear_x = pose_increment[0] #meters/second
twist.linear_y = pose_increment[1] #meters/second
twist.linear_z = pose_increment[2] #meters/second
twist.angular_x = pose_increment[3] #deg/second
twist.angular_y = pose_increment[4] #deg/second
twist.angular_z = pose_increment[5] #deg/second
base.SendTwistCommand(command)
# Let time for twist to be executed
#time.sleep(5)
#print ("Stopping the robot...")
#base.Stop()
#time.sleep(1)
return True
def joint_speed_command(base_client_service, j_id, value):
speeds = Base_pb2.JointSpeeds()
act_count = base_client_service.GetActuatorCount()
js = speeds.joint_speeds.add()
js.joint_identifier = j_id
js.value = value
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 example_send_joint_speeds(e0,base,speeds=[-18, 0, 0, 0, 0, 0, 0],timer=10):
joint_speeds = Base_pb2.JointSpeeds()
# actuator_count = base.GetActuatorCount().count
# The 7DOF robot will spin in the same direction for 10 seconds
# if actuator_count == 7:
# speeds = [-18, 0, 0, 0, 0, 0, 0]
i = 0
for speed in speeds:
joint_speed = joint_speeds.joint_speeds.add()
joint_speed.joint_identifier = i
joint_speed.value = speed
joint_speed.duration = 0
i = i + 1
#print ("Sending the joint speeds for 10 seconds...")
base.SendJointSpeedsCommand(joint_speeds)
t0=time.time()
if timer==0:
return True #robot ist still running and stopped in parent function
else:
while time.time()-t0<timer and e0.is_set()==False: #success_flag_shared.value==False:
time.sleep(0.1)
print ("Stopping the robot")
base.Stop()
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
# 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: failed to communicate")
return False
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_error_management(base):
try:
base.CreateUserProfile(Base_pb2.FullUserProfile())
except KServerException as ex:
# Get error and sub error codes
error_code = ex.get_error_code()
sub_error_code = ex.get_error_sub_code()
print("Error_code:{0} , Sub_error_code:{1} ".format(error_code, sub_error_code))
print("Caught expected error: {0}".format(ex))
def set_joint_speeds(base,j_speed_setting=5):
#The Function does nothing ...
#j_speed_setting=5 #degrees/second
joint_speeds_=Base_pb2.JointSpeeds()
joint_speeds_.duration=0
for i0 in range(7):
axis0=joint_speeds_.joint_speeds.add()
axis0.joint_identifier=i0
axis0.value=j_speed_setting
axis0.duration=0
base.SendJointSpeedsCommand(joint_speeds_)
def example_manipulation_protobuf_object():
# Messages are the main element in Google Protocol Buffer in the same way classes are to Python. You need a message
# to make a workable object. A message can contain many kind of elements. We have already
# covered the scalar value and in this section we are going to cover the message.
#
# A message can make a reference to another message to make more comprehensive element.
# For this example we'll use the FullUserProfile and UserProfile messages.
# message FullUserProfile {
# UserProfile user_profile = 1; //User Profile, which includes username.
# string password = 2; //User's password
# }
# message UserProfile {
# Kinova.Api.Common.UserProfileHandle handle = 1; // User handle (no need to be set)
# string username = 2; // username, which is used to connect to robot (or via Web App login)
# string firstname = 3; // user first name
# string lastname = 4; // user last name
# string application_data = 5; // other application data (used by Web App)
# }
# https://developers.google.com/protocol-buffers/docs/proto3#simple
full_user_profile = Base_pb2.FullUserProfile()
# Now we'll add data to the scalar
full_user_profile.password = "******"
# Now I want to work with the user profile attribute which is a message itself.
# Since user profile is a message you can use the '.' to access
# these attributes.
full_user_profile.user_profile.username = "******"
full_user_profile.user_profile.firstname = "Johnny"
full_user_profile.user_profile.lastname = "Cash"
# Another basic element is the enum. Enum is directly available from
# the message - no need to use the enum 'message'.
# Here's an example:
# enum LimitationType {
# UNSPECIFIED_LIMITATION = 0; // unspecified limitation
# FORCE_LIMITATION = 1; // force limitation
# ACCELERATION_LIMITATION = 2; // acceleration limitation
# VELOCITY_LIMITATION = 3; // velocity limitation
# }
# message CartesianLimitation {
# LimitationType type = 1; // limitation type
# }
# https://developers.google.com/protocol-buffers/docs/proto3#enum
cartesianLimitation = Base_pb2.CartesianLimitation
cartesianLimitation.type = Base_pb2.FORCE_LIMITATION
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
