self.refresh()
#def calibrate(self, point):
# q = (0.566222,0.423455,0.424871,0.5653)
# R = so3.from_quaternion(q)
# t = (0.228665,0.0591513,0.0977748)
# T = (R, t)
# newPoint = se3.apply(T,point)
# 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)
v = [0.0]*7
v[1] = 1.0
robot.left_limb.velocityCommand(v);
elif c=='m':
v = [0.0]*7
v[1] = 1.0
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)