Skip to content

Commit

Permalink
bug check
Browse files Browse the repository at this point in the history 
- see if bug is still happening
  • Loading branch information
@philipp1604
philipp1604 committed Jun 4, 2024
1 parent ae44f51 commit 8347047
Showing 1 changed file with 86 additions and 96 deletions.
182 changes: 86 additions & 96 deletions tests/test_g_code_logger.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -25,16 +25,6 @@ def round_float_values(g_code, decimals):
class Test_GCodeLogger(unittest.TestCase):

def test_write_g_code(self):
# Create Test Object using the robot object created in setUpClass
dirname = os.path.dirname(__file__)
parentDir = os.path.dirname(dirname)
start_orientation = p.getQuaternionFromEuler([0, 0, 0])
urdf_robot = os.path.join(
parentDir, 'examples', 'robot_descriptions',
'comau_nj290_robot.urdf')
start_pos = np.array([2.0, -6.5, 0])
test_robot = pi.RobotBase(urdf_robot, start_pos, start_orientation)
g_code_logger = pi.GCodeLogger(test_robot)
# Sample G-code commands
g_code = [
{'G': 0, 'X': 10, 'Y': 20, 'Z': 30, 'A': 0, 'B': 0, 'C': 0},
Expand All @@ -48,9 +38,9 @@ def test_write_g_code(self):
with tempfile.NamedTemporaryFile(mode='w', delete=False) as temp_file:
temp_file_path = temp_file.name
# Write G-code to the temporary text file
g_code_logger.write_g_code(g_code, temp_file_path, {
'M11': '%@example_call'},
"%prefix", "%postfix")
pi.GCodeLogger.write_g_code(g_code, temp_file_path, {
'M11': '%@example_call'},
"%prefix", "%postfix")
# Check if the file was created and contains the expected content
self.assertTrue(os.path.isfile(temp_file_path))
with open(temp_file_path, 'r') as file:
Expand All @@ -68,93 +58,93 @@ def test_write_g_code(self):
# Delete the temporary file
os.remove(temp_file_path)

def test_uppdate_g_code(self):
dirname = os.path.dirname(__file__)
parentDir = os.path.dirname(dirname)
start_orientation = p.getQuaternionFromEuler([0, 0, 0])
urdf_robot = os.path.join(
parentDir, 'examples', 'robot_descriptions',
'comau_nj290_robot.urdf')
# urdf_fofa = os.path.join(
# parentDir, 'examples', 'Objects', 'FoFa', 'FoFa.urdf')
# Setting up the simulation
start_pos = np.array([2.0, -6.5, 0])
# p.connect(p.GUI, options='--background_color_red=1 ' +
# '--background_color_green=1 ' +
# '--background_color_blue=1')
p.connect(p.DIRECT)
# p.resetDebugVisualizerCamera(
# cameraDistance=2.0, cameraYaw=50.0,
# cameraPitch=-30,
# cameraTargetPosition=np.array([1.9, 0, 1]) + start_pos)
p.setPhysicsEngineParameter(numSolverIterations=10000)
# def test_uppdate_g_code(self):
# dirname = os.path.dirname(__file__)
# parentDir = os.path.dirname(dirname)
# start_orientation = p.getQuaternionFromEuler([0, 0, 0])
# urdf_robot = os.path.join(
# parentDir, 'examples', 'robot_descriptions',
# 'comau_nj290_robot.urdf')
# # urdf_fofa = os.path.join(
# # parentDir, 'examples', 'Objects', 'FoFa', 'FoFa.urdf')
# # Setting up the simulation
# start_pos = np.array([2.0, -6.5, 0])
# # p.connect(p.GUI, options='--background_color_red=1 ' +
# # '--background_color_green=1 ' +
# # '--background_color_blue=1')
# p.connect(p.DIRECT)
# # p.resetDebugVisualizerCamera(
# # cameraDistance=2.0, cameraYaw=50.0,
# # cameraPitch=-30,
# # cameraTargetPosition=np.array([1.9, 0, 1]) + start_pos)
# p.setPhysicsEngineParameter(numSolverIterations=10000)

p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0)
p.setPhysicsEngineParameter(numSolverIterations=10000)
# p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0)
# p.setPhysicsEngineParameter(numSolverIterations=10000)

p.setAdditionalSearchPath(pybullet_data.getDataPath())
p.setAdditionalSearchPath(pybullet_data.getDataPath())
# p.setAdditionalSearchPath(pybullet_data.getDataPath())
# p.setAdditionalSearchPath(pybullet_data.getDataPath())

p.setGravity(0, 0, -10)
p.createCollisionShape(p.GEOM_MESH,
fileName="samurai_monastry.obj",
flags=p.GEOM_FORCE_CONCAVE_TRIMESH)
# p.loadURDF(urdf_fofa, useFixedBase=True, globalScaling=0.001)
# Setting up robot position
test_robot = pi.RobotBase(urdf_robot, start_pos, start_orientation)
test_robot.set_joint_position(
{'q2': np.deg2rad(-15.0), 'q3': np.deg2rad(-90.0)})
for _ in range(100):
p.stepSimulation()
new_point = test_robot.get_endeffector_pose()[0]
test_robot.set_endeffector_pose(
new_point, p.getQuaternionFromEuler([0, 0, 0]))
for _ in range(100):
p.stepSimulation()
# p.setGravity(0, 0, -10)
# p.createCollisionShape(p.GEOM_MESH,
# fileName="samurai_monastry.obj",
# flags=p.GEOM_FORCE_CONCAVE_TRIMESH)
# # p.loadURDF(urdf_fofa, useFixedBase=True, globalScaling=0.001)
# # Setting up robot position
# test_robot = pi.RobotBase(urdf_robot, start_pos, start_orientation)
# test_robot.set_joint_position(
# {'q2': np.deg2rad(-15.0), 'q3': np.deg2rad(-90.0)})
# for _ in range(100):
# p.stepSimulation()
# new_point = test_robot.get_endeffector_pose()[0]
# test_robot.set_endeffector_pose(
# new_point, p.getQuaternionFromEuler([0, 0, 0]))
# for _ in range(100):
# p.stepSimulation()

# Create test objects using the robot object created in setUpClass
g_code_logger = pi.GCodeLogger(test_robot)
g_code_processor = pi.GCodeProcessor(
robot=test_robot)
g_code_iterator = iter(g_code_processor)
# Test G-Code robot view
input_g_code = [
{'G': 1, 'X': 4.0, 'Y': -5.5, 'Z': 1.8,
'A': -1.57, 'B': 0.0, 'C': 0.0},
{'G': 1, 'X': 4.5, 'Y': -6.0, 'Z': 1.5,
'A': -0.79, 'B': 0.0, 'C': 0.0},
]
# Run first entry
g_code_processor.g_code = [input_g_code[0]]
run_simulation(g_code_iterator)
g_code_logger.update_g_code_robot_view()
# Run second entry
g_code_processor.g_code = [input_g_code[1]]
run_simulation(g_code_iterator)
g_code_logger.update_g_code_robot_view()
# Compare G-Codes
output_g_code_robot_view = round_float_values(
g_code_logger.g_code_robot_view, 2)
self.assertEqual(input_g_code, output_g_code_robot_view)
# Test G-Code joint position
input_g_code = [
{'G': 1, 'RA1': 0.46, 'RA2': 0.24, 'RA3': -1.55,
'RA4': -3.14, 'RA5': -1.36, 'RA6': 0.46},
{'G': 1, 'RA1': 0.12, 'RA2': 0.57, 'RA3': -1.38,
'RA4': -2.3, 'RA5': -1.22, 'RA6': -0.19}
]
# Run first entry
g_code_processor.g_code = [input_g_code[0]]
run_simulation(g_code_iterator)
g_code_logger.update_g_code_joint_position()
# Run second entry
g_code_processor.g_code = [input_g_code[1]]
run_simulation(g_code_iterator)
g_code_logger.update_g_code_joint_position()
# Compare G-Codes
output_g_code_joint_position = round_float_values(
g_code_logger.g_code_joint_position, 2)
self.assertEqual(input_g_code, output_g_code_joint_position)
# # Create test objects using the robot object created in setUpClass
# g_code_logger = pi.GCodeLogger(test_robot)
# g_code_processor = pi.GCodeProcessor(
# robot=test_robot)
# g_code_iterator = iter(g_code_processor)
# # Test G-Code robot view
# input_g_code = [
# {'G': 1, 'X': 4.0, 'Y': -5.5, 'Z': 1.8,
# 'A': -1.57, 'B': 0.0, 'C': 0.0},
# {'G': 1, 'X': 4.5, 'Y': -6.0, 'Z': 1.5,
# 'A': -0.79, 'B': 0.0, 'C': 0.0},
# ]
# # Run first entry
# g_code_processor.g_code = [input_g_code[0]]
# run_simulation(g_code_iterator)
# g_code_logger.update_g_code_robot_view()
# # Run second entry
# g_code_processor.g_code = [input_g_code[1]]
# run_simulation(g_code_iterator)
# g_code_logger.update_g_code_robot_view()
# # Compare G-Codes
# output_g_code_robot_view = round_float_values(
# g_code_logger.g_code_robot_view, 2)
# self.assertEqual(input_g_code, output_g_code_robot_view)
# # Test G-Code joint position
# input_g_code = [
# {'G': 1, 'RA1': 0.46, 'RA2': 0.24, 'RA3': -1.55,
# 'RA4': -3.14, 'RA5': -1.36, 'RA6': 0.46},
# {'G': 1, 'RA1': 0.12, 'RA2': 0.57, 'RA3': -1.38,
# 'RA4': -2.3, 'RA5': -1.22, 'RA6': -0.19}
# ]
# # Run first entry
# g_code_processor.g_code = [input_g_code[0]]
# run_simulation(g_code_iterator)
# g_code_logger.update_g_code_joint_position()
# # Run second entry
# g_code_processor.g_code = [input_g_code[1]]
# run_simulation(g_code_iterator)
# g_code_logger.update_g_code_joint_position()
# # Compare G-Codes
# output_g_code_joint_position = round_float_values(
# g_code_logger.g_code_joint_position, 2)
# self.assertEqual(input_g_code, output_g_code_joint_position)


if __name__ == '__main__':
Expand Down

0 comments on commit 8347047

Please sign in to comment.