import math

DEBUG = True

# IP of the robot.
UR5_IP = "129.241.187.119"

# Enables force constraints on the tool. Note: Setting this to "True" involves
# polling the UR5 through a real-time monitor that runs at 125Hz. If the controller
# runs slow, try disabling this first.
USE_FORCE_MONITOR = True

# Control parameters.
VELOCITY = 0.1
ACCELERATION = 0.5
DECELERATION = 0.5

# Block parameters
BLOCK_DIM = 0.1 #TODO measure

# Table polar parameters
TABLE_QUADRANT = 3 # In range [0..4). Quadrant 0 is along the x-axis of the robot.
TABLE_ORIGO_OFFSET = -0.25 # TODO measure
TABLE_EDGE_LEFT = -math.pi/4
TABLE_EDGE_RIGHT = math.pi/4
R_MIN = 0.6 # relative to TABLE center
R_MAX = 0.9 # relative to TABLE center

# Table height parameters (TODO CHECK THESE)
Z_TABLE = -0.337
Z_MIN = Z_TABLE + BLOCK_DIM/2
Z_MAX = Z_MIN + 5*BLOCK_DIM


# Home quadrant joint configuration.
# We need this expressed in joint space to ensure that the robot does not
# collide with itself when moving to the base configuration.
QUADRANT_JOINT_CONFIG = [TABLE_QUADRANT*math.pi/2,
			   	   		 -3*math.pi/4,
			   	   		 -3*math.pi/4,
			   	   		 -math.pi/2,
			   	  		 -math.pi/2,
			  	  		 math.pi]