def __init__(self, sim_path="./"):
self.oreo = oreo.Oreo_Robot(True, False, sim_path, "assembly.urdf",
False)
self.oreo.InitModel()
self.oreo.InitManCtrl()
# self.oreo.get_max_min_yaw_pitch_values()
# read the interpolator function from pickle file
b = self.oreo.read_interpolator_functions()
if b == 0:
a = self.oreo.read_oreo_yaw_pitch_actuator_data()
if a == 0:
print("Building scan data takes minutes ....")
self.oreo.build_oreo_scan_yaw_pitch_actuator_data()
self.oreo.produce_interpolators()
return
if (cnt == 10):
cnt += 1
torque *= -1
force *= -1
cnt = 0
robot.ControlActJoints(ctrl_dict)
cnt += 1
keys = robot.GetKeyEvents()
if 'q' in keys:
# quit
break
if 'd' in keys:
robot.PrintConstraintDynamics()
if 'p' in keys:
robot.GetLinkPosOrn('right_eye_joint')
robot = o.Oreo_Robot(True, True, "/home/oreo/Documents/oreo_sim/oreo/sim",
"assembly.urdf", True)
robot.InitModel()
man_ctrl = True
if man_ctrl:
ManCtrl(robot)
else:
TorqueCtrl(robot)
robot.Cleanup()
i,
computeForwardKinematics=True)
print("Position {} Orientation {}".format(state[4], state[5]))
print("XXXXXXX End of joint information for {}\n".format(
jointInfo[1].decode('UTF-8')))
new_direction = np.array([0.5, 0.5, 0.0])
new_direction = new_direction / np.linalg.norm(new_direction)
myrobot.compute_IK_for_actuators(new_direction)
return
if __name__ == "__main__":
robot = oreo.Oreo_Robot(True, True, "./", "assembly.urdf", True)
#robot = oreo.Oreo_Robot(True, True, "/home/oreo/Documents/oreo_sim/oreo/sim", "assembly.urdf", True)
robot.InitModel()
print(robot.GetJointNames())
robot.InitManCtrl()
robot.RegKeyEvent(['c', 'q', 'p'])
#test_joint_info(robot)
#my_results = test_IK(robot)
#read table data from pickle file
a = robot.read_oreo_yaw_pitch_actuator_data()
if a == 0:
print("Building scan data takes minutes ....")
robot.build_oreo_scan_yaw_pitch_actuator_data()