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 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):
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 twist_command(base_client_service, x, y, z, tx, ty, tz):
command = Base_pb2.TwistCommand()
command.mode = Base_pb2.UNSPECIFIED_TWIST_MODE
command.duration = 2 # Unlimited time to execute
twist = command.twist
twist.linear_x = x
twist.linear_y = y
twist.linear_z = z
twist.angular_x = tx
twist.angular_y = ty
twist.angular_z = tz
base_client_service.SendTwistCommand(command)
def move_to_protection_zone(base):
command = Base_pb2.TwistCommand()
command.reference_frame = Base_pb2.CARTESIAN_REFERENCE_FRAME_BASE
command.duration = 0
twist = command.twist
twist.linear_x = 0.05
twist.linear_y = 0
twist.linear_z = 0
twist.angular_x = 0
twist.angular_y = 0
twist.angular_z = 0
print ("Moving towards the protection zone for 4 seconds...")
base.SendTwistCommand(command)
# Let time for twist to be executed
time.sleep(4)
print ("Stopping the robot...")
base.Stop()
time.sleep(1)
