def setupController(self, physical=False):
simworld = self.makeWorld()
planworld = self.makeWorld()
visualizer = None
sim = None
if not physical:
#virtual simulation
sim = robotsim.Simulator(simworld)
low_level_controller = LowLevelController(simworld.robot(0),sim.controller(0),sim)
self.controller = PickingController(simworld, planworld, low_level_controller)
myHelper = Helper(self)
visualizer = CustomGLViewer.CustomGLViewer(simworld,planworld, low_level_controller, sim, helper = myHelper)
self.command_Queue = visualizer.command_queue
else:
#actually connected to the robot
motion.setup(mode=config.mode,klampt_model=config.klampt_model,libpath="./")
res = motion.robot.startup()
if not res:
print "Error starting up Motion Module"
exit(1)
time.sleep(0.1)
q = motion.robot.getKlamptSensedPosition()
simworld.robot(0).setConfig(q)
planworld.robot(0).setConfig(q)
low_level_controller = PhysicalLowLevelController(simworld.robot(0))
self.controller = PickingController(simworld, planworld, low_level_controller)
myHelper = Helper(self)
visualizer = CustomGLViewer.CustomGLViewer(simworld,planworld, low_level_controller, helper=myHelper)
self.command_Queue = visualizer.command_queue
sim = robotsim.Simulator(simworld)
self.simworld = simworld
self.fake_controller = LowLevelController(simworld.robot(0),sim.controller(0),sim)
self.visualizer = visualizer
visualizer.run()
def main():
global tx, ty, tz, rx, ry, rz
klampt_model = os.path.join(os.path.expanduser("~"),
"Klampt/data/robots/baxter_col.rob")
mode = "physical"
robot = motion.setup(mode=mode, klampt_model=klampt_model)
res = robot.startup()
run_event_reader()
while (True):
print[rx, ry, rz], [tx, ty, tz]
robot.left_ee.velocityCommand([rx, ry, rz], [tx, ty, tz])
print(robot.left_ee.commandedVelocity())
tx = 0 # ABS_X event code: 0
ty = 0 # ABS_Y event code: 1
tz = 0 # ABS_Z event code: 2
rx = 0 # ABS_RX event code: 3
ry = 0 # ABS_RY event code: 4
rz = 0 # ABS_RZ event code: 5
time.sleep(0.05) # set rate to 20Hz
def main():
S_MN = 0 ## Slider min
S_MX = 250 ## Slider max
## Fort number to automatically connect to. If set to -1, user will be prompted for a port.
portOverride = 0;
## Open the serial port that the Ardino is connected to
if (portOverride == -1):
port = raw_input("Enter Arduino serial port number (/dev/ttyACM#): ")
ser = serial.Serial("/dev/ttyACM" + port, 9600)
else:
ser = serial.Serial("/dev/ttyACM" + str(portOverride), 9600)
## Startup
print "Testing APC Motion..."
robot = motion.setup(libpath="./",klampt_model="/home/motion/Klampt/data/robots/baxter_col.rob")
print "Starting up..."
res = robot.startup()
print "Start up result:",res
if res:
print "Is robot started?",robot.isStarted()
print
## Infinite read/send loop
while True:
message = ser.readline()
message = message[:-2] ## Remove new line characters
sliderRaw = str.split(message,",")
## Parse check before converting to int
if len(sliderRaw) == 7:
try:
sliderVals = [float(n) for n in sliderRaw]
print sliderVals
jointAngles = [0,0,0,0,0,mapVal(sliderVals[0],S_MN,S_MX,-1.5708,2.094),0]
print jointAngles
robot.right_mq.setRamp(jointAngles)
except ValueError:
pass
# x = newPoint[0]
# y = newPoint[1]
# z = newPoint[2]
# return (-y,x,z)
if __name__ == "__main__":
config.parse_args()
print "widget_control.py: manually sends configurations to the Motion module"
print "Press [space] to send milestones. Press q to exit."
print
print "Loading APC Motion model", config.klampt_model
motion.setup(
mode=config.mode,
klampt_model=config.klampt_model,
libpath="./",
)
res = motion.robot.startup()
if not res:
print "Error starting up APC Motion"
exit(1)
time.sleep(0.1)
world = WorldModel()
res = world.readFile(config.klampt_model)
if not res:
raise RuntimeError("Unable to load APC Motion model " + fn)
viewer = MyGLViewer(world)
viewer.run()
break
start = self.perturb(start, 0.1)
robot.setConfig(start)
return res
if __name__ == "__main__":
config.parse_args()
print "widget_control.py: manually sends configurations to the Motion module"
print "Press [space] to send milestones. Press q to exit."
print
print "Loading APC Motion model", config.klampt_model
motion.setup(mode=config.mode,
klampt_model=config.klampt_model,
libpath="./",
server_addr=config.motion_server_addr)
res = motion.robot.startup()
if not res:
print "Error starting up APC Motion"
exit(1)
time.sleep(0.1)
world = WorldModel()
res = world.readFile(config.klampt_model)
if not res:
raise RuntimeError("Unable to load APC Motion model " + fn)
viewer = MyGLViewer(world)
viewer.run()
from Motion import motion
import sys
print "Testing APC Motion..."
robot = motion.setup(libpath="./",klampt_model="/home/motion/Klampt/data/robots/baxter_col.rob")
print "Starting up..."
res = robot.startup()
print "Start up result:",res
if res:
print "Is robot started?",robot.isStarted()
print
gripper = sys.argv[1]
if gripper == 'left':
print "Enabled?",robot.left_gripper.enabled()
robot.left_gripper.close()
else:
print "Enabled?",robot.right_gripper.enabled()
robot.right_gripper.close()
time.sleep(1.0)
exit(0)
def keyboardfunc(self,c,x,y):
#Put your keyboard handler here
#the current example toggles simulation / movie mode
print c,"pressed"
if c == 'q':
motion.robot.shutdown()
exit(0)
glutPostRedisplay()
if __name__ == "__main__":
print "motion_template.py: sets up a visualizer for testing Motion"
print "control loops. Press q to exit."
print
print "Loading APC Motion model",klampt_model
motion.setup(mode=mode,klampt_model=klampt_model,libpath="./")
res = motion.robot.startup()
if not res:
print "Error starting up APC Motion"
exit(1)
time.sleep(0.1)
world = WorldModel()
res = world.readFile(klampt_model)
if not res:
raise RuntimeError("Unable to load APC Motion model "+fn)
viewer = MyGLViewer(world)
viewer.run()
PATH_DICTIONARY = {}
WORLD = robotsim.WorldModel()
print "Loading simplified Baxter model..."
WORLD.loadElement(os.path.join(MODEL_DIR, KLAMPT_MODEL))
try:
file = open(RESOURCE_DIR + 'data.json', 'rw')
PATH_DICTIONARY = json.load(file)
file.close()
except:
raise Exception('Path Dictionary failed to load')
# Connect to Baxter
robot = motion.setup(mode="physical",
libpath=COMMON_DIR,
klampt_model=MODEL_DIR + KLAMPT_MODEL)
res = robot.startup()
if not res:
print "Error connecting to robot"
exit()
time.sleep(0.1)
ROBOT = WORLD.robot(0)
CONTROLLER = PhysicalLowLevelController(ROBOT)
print("Initializing node... ")
rospy.init_node('baxter_klampt_controller')
print("Getting robot state... ")
rs = baxter_interface.RobotEnable(CHECK_VERSION)
init_state = rs.state().enabled
robot.left_mq.setRamp(v,0.5);
v[2] = -1.5
v[3] = 2.0
robot.left_mq.appendCubic(2.0,v,[0.0]*7);
elif c=='s':
robot.stopMotion();
if __name__ == '__main__':
config.parse_args()
print "motion_testing.py: various random tests of the motion module."""
print "Press q to exit."
print
print "Loading Motion Module model",config.klampt_model
motion.setup(mode=config.mode,klampt_model=config.klampt_model,libpath="./")
res = motion.robot.startup()
if not res:
print "Error starting up Motion Module"
exit(1)
time.sleep(0.1)
world = WorldModel()
res = world.readFile(config.klampt_model)
if not res:
raise RuntimeError("Unable to load Klamp't model "+fn)
program = EbolabotTestUIViewer(world)
program.run()
motion.robot.shutdown()
#================================================================
# End of Imports
#============================================================
# configuration variables
MODEL_DIR = "/home/saeed/Klampt/data/robots/"
LIBMOTION_DIR = "/home/saeed/catkin_ws/src/klampt/misc/"
KLAMPT_MODEL = "baxter_col.rob"
#============================================================
# paths/directories variables
if __name__ == "__main__":
"""The main loop that loads the planning / simulation models and
starts the OpenGL visualizer."""
print 'motion_controller runs!'
robot_model = MODEL_DIR + KLAMPT_MODEL
print robot_model
# Connect to Baxter
robot = motion.setup(
mode="physical", \
libpath=LIBMOTION_DIR, \
klampt_model= MODEL_DIR+KLAMPT_MODEL)
res = robot.startup()
if not res:
print "Error connecting to robot"
exit()
def start(self):
motion.setup(mode="physical", klampt_model=os.path.join(KLAMPT_MODELS_DIR, "baxter_col.rob"), libpath=LIBPATH)
motion.robot.startup()
rospy.init_node("listener", anonymous=True)
self.loop()
print "Press q to exit."
print
if __name__ == "__main__":
config.parse_args()
KLAMPT_MODEL_PATH = "../klampt_models/baxter_col.rob"
print "drive_control.py: allows driving end effectors with the Motion module"
print
print "Loading Motion Module model",KLAMPT_MODEL_PATH
motion.setup(mode=config.mode,klampt_model=KLAMPT_MODEL_PATH,libpath="./",)
res = motion.robot.startup()
if not res:
print "Error starting up Motion Module"
exit(1)
time.sleep(0.01)
print "Started Motion Module"
world = WorldModel()
res = world.readFile(KLAMPT_MODEL_PATH)
if not res:
raise RuntimeError("Unable to load Klamp't model "+KLAMPT_MODEL_PATH)
viewer = MyEEPoseProgram(world)
viewer.print_help()
viewer.run()
motion.robot.shutdown()
print "i is pressed. Take current position as start point. "
if self.takeCurrentPosition is False:
self.takeCurrentPosition = True
glutPostRedisplay()
if __name__ == "__main__":
config_physical.parse_args()
print "motion_template.py: sets up a visualizer for testing Motion"
print "control loops. Press q to exit."
print
print "Loading APC Motion model", config_physical.klampt_model
print "config.mode =", config_physical.mode
robot = motion.setup(mode=config_physical.mode, klampt_model=lib_path+config_physical.klampt_model, libpath=lib_path)
res = robot.startup()
if not res:
print "Error starting up APC Motion"
exit(1)
time.sleep(0.1)
world = WorldModel()
res = world.readFile(lib_path+config.klampt_model)
if not res:
raise RuntimeError("Unable to load APC Motion model ")
viewer = MyGLViewer(world)
viewer.run()
import sys; sys.path.append('.')
from Motion import motion
#manual motion setup
motion.setup(mode='kinematic',klampt_model='klampt_models/baxter_with_parallel_gripper_col.rob',libpath='./')
#automatic motion setup
#from Motion import config
#config.setup(parse_sys=False)
motion.robot.startup()
try:
while True:
raw_input("Press enter to print sensed left arm positions > ")
print motion.robot.left_limb.sensedPosition()
except KeyboardInterrupt:
print "Shutting down..."
print "Head pan center"
motion.robot.head.setPan(0,0.25)
elif c == '3':
print "Head pan right"
motion.robot.head.setPan(-1.57,0.25)
else:
GLWidgetProgram.keyboardfunc(self,c,x,y)
self.refresh()
if __name__ == "__main__":
config.parse_args()
print "widget_control.py: manually sends configurations to the Motion module"
print "Press [space] to send milestones. Press q to exit."
print
print "Loading Motion Module model",config.klampt_model
motion.setup(mode=config.mode,klampt_model=config.klampt_model,libpath="./",server_addr=EbolabotSystemConfig.get_ip('motion_computer_ip')[0])
res = motion.robot.startup()
if not res:
print "Error starting up Motion Module"
exit(1)
time.sleep(0.1)
world = WorldModel()
res = world.readFile(config.klampt_model)
if not res:
raise RuntimeError("Unable to load Klamp't model "+config.klampt_model)
viewer = MyGLViewer(world)
viewer.run()
