diff --git a/python/examples/clik.py b/python/examples/clik.py
index 651e8c31f9412d4d2de12bc44778cf3c6b01d68c..622e719e90fb1756a205d959c659555605caadf5 100644
--- a/python/examples/clik.py
+++ b/python/examples/clik.py
@@ -39,8 +39,9 @@ def get_args():
help="whether you want to visualize the manipulator and workspace with meshcat", default=False)
parser.add_argument('--real-time-plotting', action=argparse.BooleanOptionalAction,
help="whether you want to have some real-time matplotlib graphs (parts of log_dict you select)", default=False)
- parser.add_argument('--gripper', action=argparse.BooleanOptionalAction, \
- help="whether you're using the gripper", default=False)
+ parser.add_argument('--gripper', type=str, \
+ help="gripper you're using (no gripper is the default)",
+ default="none", choices=['none', 'robotiq', 'onrobot'])
parser.add_argument('--goal-error', type=float, \
help="the final position error you are happy with", default=1e-2)
parser.add_argument('--max-iterations', type=int, \
diff --git a/python/examples/drawing_from_input_drawing.py b/python/examples/drawing_from_input_drawing.py
index 2bf8e0f4efc9e18467a79152882be5b973c28fbd..7c30f64ff7502ca661f7b37d898a17d8a1815f38 100644
--- a/python/examples/drawing_from_input_drawing.py
+++ b/python/examples/drawing_from_input_drawing.py
@@ -50,8 +50,9 @@ def getArgs():
help="whether you want to visualize the manipulator and workspace with meshcat", default=False)
parser.add_argument('--real-time-plotting', action=argparse.BooleanOptionalAction,
help="whether you want to have some real-time matplotlib graphs (parts of log_dict you select)", default=False)
- parser.add_argument('--gripper', action=argparse.BooleanOptionalAction, \
- help="whether you're using the gripper", default=False)
+ parser.add_argument('--gripper', type=str, \
+ help="gripper you're using (no gripper is the default)",
+ default="none", choices=['none', 'robotiq', 'onrobot'])
parser.add_argument('--acceleration', type=float, \
help="robot's joints acceleration. scalar positive constant, \
max 1.7, and default 0.4. \
diff --git a/python/examples/oc_for_ik.py b/python/examples/oc_for_ik.py
new file mode 100644
index 0000000000000000000000000000000000000000..36ce607997405ea29e755c6422305b9c337f15b5
--- /dev/null
+++ b/python/examples/oc_for_ik.py
@@ -0,0 +1,112 @@
+import numpy as np
+import time
+import argparse
+from functools import partial
+from ur_simple_control.managers import ControlLoopManager, RobotManager
+# TODO: implement
+from ur_simple_control.optimal_control.optimal_control import IKOCP
+# TODO: implement
+from ur_simple_control.basics.basics import jointTrajFollowingPID
+from ur_simple_control.util.logging_utils import saveLog
+import pinocchio as pin
+
+
+"""
+Every imaginable magic number, flag and setting is put in here.
+This way the rest of the code is clean.
+If you want to know what these various arguments do, just grep
+though the code to find them (but replace '-' with '_' in multi-word arguments).
+All the sane defaults are here, and you can change any number of them as an argument
+when running your program. If you want to change a drastic amount of them, and
+not have to run a super long commands, just copy-paste this function and put your
+own parameters as default ones.
+NOTE1: the args object obtained from args = parser.get_args()
+is pushed all around the rest of the code (because it's tidy), so don't change their names.
+NOTE2: that you need to copy-paste and add aguments you need
+to the script you will be writing. This is kept here
+only as a convenient reference point.
+"""
+def get_args():
+ parser = argparse.ArgumentParser(description='Run closed loop inverse kinematics \
+ of various kinds. Make sure you know what the goal is before you run!',
+ formatter_class=argparse.ArgumentDefaultsHelpFormatter)
+ parser.add_argument('--simulation', action=argparse.BooleanOptionalAction,
+ help="whether you are running the UR simulator", default=False)
+ parser.add_argument('--robot-ip', type=str,
+ help="robot's ip address (only needed if running on the real robot)", \
+ default="192.168.1.102")
+ parser.add_argument('--pinocchio-only', action=argparse.BooleanOptionalAction,
+ help="whether you want to just integrate with pinocchio", default=False)
+ parser.add_argument('--visualize-manipulator', action=argparse.BooleanOptionalAction,
+ help="whether you want to visualize the manipulator and workspace with meshcat", default=False)
+ parser.add_argument('--real-time-plotting', action=argparse.BooleanOptionalAction,
+ help="whether you want to have some real-time matplotlib graphs (parts of log_dict you select)", default=False)
+ parser.add_argument('--gripper', action=argparse.BooleanOptionalAction, \
+ help="whether you're using the gripper", default=False)
+ parser.add_argument('--goal-error', type=float, \
+ help="the final position error you are happy with", default=1e-2)
+ parser.add_argument('--max-iterations', type=int, \
+ help="maximum allowable iteration number (it runs at 500Hz)", default=100000)
+ parser.add_argument('--acceleration', type=float, \
+ help="robot's joints acceleration. scalar positive constant, max 1.7, and default 0.4. \
+ BE CAREFUL WITH THIS. the urscript doc says this is 'lead axis acceleration'.\
+ TODO: check what this means", default=0.3)
+ parser.add_argument('--speed-slider', type=float,\
+ help="cap robot's speed with the speed slider \
+ to something between 0 and 1, 0.5 by default \
+ BE CAREFUL WITH THIS.", default=0.5)
+ parser.add_argument('--tikhonov-damp', type=float, \
+ help="damping scalar in tikhonov regularization", default=1e-3)
+ parser.add_argument('--minimum-detectable-force-norm', type=float, \
+ help="we need to disregard noise to converge despite filtering. \
+ a quick fix is to zero all forces of norm below this argument threshold.",
+ default=3.0)
+ # TODO add the rest
+ parser.add_argument('--clik-controller', type=str, \
+ help="select which click algorithm you want", \
+ default='dampedPseudoinverse', choices=['dampedPseudoinverse', 'jacobianTranspose', 'invKinmQP'])
+ # maybe you want to scale the control signal
+ parser.add_argument('--controller-speed-scaling', type=float, \
+ default='1.0', help='not actually_used atm')
+ parser.add_argument('--debug-prints', action=argparse.BooleanOptionalAction, \
+ help="print some debug info", default=False)
+ parser.add_argument('--save-log', action=argparse.BooleanOptionalAction, \
+ help="save log data folder in whatever directory you called this in. if it doesn't exists there, it's saved to /tmp/data", default=False)
+ parser.add_argument('--alpha', type=float, \
+ help="force feedback proportional coefficient", \
+ default=0.01)
+ parser.add_argument('--beta', type=float, \
+ help="low-pass filter beta parameter", \
+ default=0.01)
+ parser.add_argument('--past-window-size', type=int, \
+ help="how many timesteps of past data you want to save", default=5)
+
+ args = parser.parse_args()
+ if args.gripper and args.simulation:
+ raise NotImplementedError('Did not figure out how to put the gripper in \
+ the simulation yet, sorry :/ . You can have only 1 these flags right now')
+ return args
+
+if __name__ == "__main__":
+ args = get_args()
+ robot = RobotManager(args)
+ #Mgoal = robot.defineGoalPointCLI()
+ Mgoal = pin.SE3.Identity()
+ Mgoal.translation = np.array([0.3,0.3,0.3])
+ traj = IKOCP(robot, Mgoal)
+ exit()
+
+ #log_dict, final_iteration = compliantMoveL(args, robot, Mgoal)
+ if not args.pinocchio_only:
+ print("stopping via freedrive lel")
+ robot.rtde_control.freedriveMode()
+ time.sleep(0.5)
+ robot.rtde_control.endFreedriveMode()
+
+ if args.visualize_manipulator:
+ robot.killManipulatorVisualizer()
+
+ if args.save_log:
+ saveLog(log_dict, final_iteration, args)
+ #loop_manager.stopHandler(None, None)
+
diff --git a/python/examples/point_impedance_control.py b/python/examples/point_impedance_control.py
index 58ac4d710d83d7e85e691c37973e8ce7ca263a0c..8167bcc184d9bd23c8bbdb040cb87fcdea60a5f5 100644
--- a/python/examples/point_impedance_control.py
+++ b/python/examples/point_impedance_control.py
@@ -44,8 +44,9 @@ def getArgs():
help="whether you want to visualize the manipulator and workspace with meshcat", default=False)
parser.add_argument('--real-time-plotting', action=argparse.BooleanOptionalAction,
help="whether you want to have some real-time matplotlib graphs (parts of log_dict you select)", default=False)
- parser.add_argument('--gripper', action=argparse.BooleanOptionalAction, \
- help="whether you're using the gripper", default=False)
+ parser.add_argument('--gripper', type=str, \
+ help="gripper you're using (no gripper is the default)",
+ default="none", choices=['none', 'robotiq', 'onrobot'])
parser.add_argument('--acceleration', type=float, \
help="robot's joints acceleration. scalar positive constant, \
max 1.7, and default 0.4. \
diff --git a/python/ur_simple_control.egg-info/SOURCES.txt b/python/ur_simple_control.egg-info/SOURCES.txt
index 958962fe06fb5f61114fbec7342e08ea2b3b8c16..8ed94185351c47000095ce13d61fc99f3f3b3f99 100644
--- a/python/ur_simple_control.egg-info/SOURCES.txt
+++ b/python/ur_simple_control.egg-info/SOURCES.txt
@@ -1,10 +1,271 @@
LICENSE.txt
README.md
+joint_trajectory.csv
+path_in_pixels.csv
setup.py
+convenience_tool_box/check_tcp_payload
+convenience_tool_box/currents.png
+convenience_tool_box/frame_validation.py
+convenience_tool_box/freedrive.py
+convenience_tool_box/ft_readings.py
+convenience_tool_box/fts.png
+convenience_tool_box/jog_example
+convenience_tool_box/measuring_stick.py
+convenience_tool_box/notes.md
+convenience_tool_box/open_close_gripper.py
+convenience_tool_box/taus.png
+convenience_tool_box/__pycache__/give_me_the_calibrated_model.cpython-310.pyc
+convenience_tool_box/__pycache__/robotiq_gripper.cpython-310.pyc
+examples/clik.py
+examples/drawing_from_input_drawing.py
+examples/joint_trajectory.csv
+examples/path_in_pixels.csv
+examples/planar_dragging_via_top_contact_force.py
+examples/point_impedance_control.py
+examples/pushing_via_friction_cones.py
+examples/test_crocoddyl_opt_ctrl.py
+examples/test_gripper.py
+examples/test_movej.py
+examples/__pycache__/robotiq_gripper.cpython-310.pyc
+examples/data/clik_run_001.pickle
+examples/data/clik_run_001_args.pickle
+examples/data/fts.png
+examples/data/joint_trajectory.csv
+examples/data/path_in_pixels.csv
+examples/old_or_experimental/clik_old.py
+examples/old_or_experimental/force_mode_api.py
+examples/old_or_experimental/forcemode_example.py
+examples/old_or_experimental/test_traj_w_movej.py
+examples/old_or_experimental/test_traj_w_speedj.py
+initial_python_solution/main.py
+initial_python_solution/make_run.py
+initial_python_solution/manipulator_visual_motion_analyzer.py
+initial_python_solution/__pycache__/make_run.cpython-310.pyc
+initial_python_solution/__pycache__/make_run.cpython-311.pyc
+initial_python_solution/arms/another_debug_dh
+initial_python_solution/arms/j2n6s300_dh_params
+initial_python_solution/arms/j2s7s300_dh_params
+initial_python_solution/arms/kinova_jaco_params
+initial_python_solution/arms/kuka_lbw_iiwa_dh_params
+initial_python_solution/arms/lbr_iiwa_jos_jedan_dh
+initial_python_solution/arms/robot_parameters2
+initial_python_solution/arms/testing_dh_parameters
+initial_python_solution/arms/ur10e_dh_parameters_from_the_ur_site
+initial_python_solution/arms/ur5e_dh
+initial_python_solution/example_code/blit_in_tk.py
+initial_python_solution/example_code/blit_test.py
+initial_python_solution/example_code/dark_background_example.py
+initial_python_solution/example_code/ellipse.py
+initial_python_solution/example_code/embedding_in_tk_sgskip.py
+initial_python_solution/example_code/manager_anim.py
+initial_python_solution/example_code/run_classic_ik_algs.py
+initial_python_solution/robot_stuff/InverseKinematics.py
+initial_python_solution/robot_stuff/drawing.py
+initial_python_solution/robot_stuff/drawing_for_anim.py
+initial_python_solution/robot_stuff/follow_curve.py
+initial_python_solution/robot_stuff/forw_kinm.py
+initial_python_solution/robot_stuff/inv_kinm.py
+initial_python_solution/robot_stuff/joint_as_hom_mat.py
+initial_python_solution/robot_stuff/utils.py
+initial_python_solution/robot_stuff/webots_api_helper_funs.py
+initial_python_solution/robot_stuff/__pycache__/InverseKinematics.cpython-310.pyc
+initial_python_solution/robot_stuff/__pycache__/InverseKinematics.cpython-311.pyc
+initial_python_solution/robot_stuff/__pycache__/drawing.cpython-310.pyc
+initial_python_solution/robot_stuff/__pycache__/drawing.cpython-311.pyc
+initial_python_solution/robot_stuff/__pycache__/drawing_for_anim.cpython-310.pyc
+initial_python_solution/robot_stuff/__pycache__/drawing_for_anim.cpython-311.pyc
+initial_python_solution/robot_stuff/__pycache__/follow_curve.cpython-310.pyc
+initial_python_solution/robot_stuff/__pycache__/follow_curve.cpython-311.pyc
+initial_python_solution/robot_stuff/__pycache__/forw_kinm.cpython-310.pyc
+initial_python_solution/robot_stuff/__pycache__/forw_kinm.cpython-311.pyc
+initial_python_solution/robot_stuff/__pycache__/inv_kinm.cpython-310.pyc
+initial_python_solution/robot_stuff/__pycache__/inv_kinm.cpython-311.pyc
+initial_python_solution/robot_stuff/__pycache__/joint_as_hom_mat.cpython-310.pyc
+initial_python_solution/robot_stuff/__pycache__/joint_as_hom_mat.cpython-311.pyc
+initial_python_solution/robot_stuff/__pycache__/utils.cpython-310.pyc
+initial_python_solution/robot_stuff/__pycache__/utils.cpython-311.pyc
+initial_python_solution/robot_stuff/__pycache__/webots_api_helper_funs.cpython-310.pyc
+initial_python_solution/robot_stuff/__pycache__/webots_api_helper_funs.cpython-311.pyc
ur_simple_control/__init__.py
ur_simple_control/managers.py
ur_simple_control.egg-info/PKG-INFO
ur_simple_control.egg-info/SOURCES.txt
ur_simple_control.egg-info/dependency_links.txt
ur_simple_control.egg-info/not-zip-safe
-ur_simple_control.egg-info/top_level.txt
\ No newline at end of file
+ur_simple_control.egg-info/top_level.txt
+ur_simple_control/__pycache__/__init__.cpython-310.pyc
+ur_simple_control/__pycache__/__init__.cpython-311.pyc
+ur_simple_control/__pycache__/__init__.cpython-312.pyc
+ur_simple_control/__pycache__/managers.cpython-310.pyc
+ur_simple_control/__pycache__/managers.cpython-311.pyc
+ur_simple_control/__pycache__/managers.cpython-312.pyc
+ur_simple_control/basics/__init__.py
+ur_simple_control/basics/basics.py
+ur_simple_control/basics/__pycache__/__init__.cpython-310.pyc
+ur_simple_control/basics/__pycache__/__init__.cpython-311.pyc
+ur_simple_control/basics/__pycache__/__init__.cpython-312.pyc
+ur_simple_control/basics/__pycache__/basics.cpython-310.pyc
+ur_simple_control/basics/__pycache__/basics.cpython-311.pyc
+ur_simple_control/basics/__pycache__/basics.cpython-312.pyc
+ur_simple_control/clik/__init__.py
+ur_simple_control/clik/clik_point_to_point.py
+ur_simple_control/clik/clik_trajectory_following.py
+ur_simple_control/clik/__pycache__/__init__.cpython-310.pyc
+ur_simple_control/clik/__pycache__/__init__.cpython-311.pyc
+ur_simple_control/clik/__pycache__/__init__.cpython-312.pyc
+ur_simple_control/clik/__pycache__/clik_point_to_point.cpython-310.pyc
+ur_simple_control/clik/__pycache__/clik_point_to_point.cpython-311.pyc
+ur_simple_control/clik/__pycache__/clik_point_to_point.cpython-312.pyc
+ur_simple_control/clik/__pycache__/clik_trajectory_following.cpython-310.pyc
+ur_simple_control/clik/__pycache__/clik_trajectory_following.cpython-311.pyc
+ur_simple_control/clik/__pycache__/clik_trajectory_following.cpython-312.pyc
+ur_simple_control/dmp/__init__.py
+ur_simple_control/dmp/dmp.py
+ur_simple_control/dmp/notes.md
+ur_simple_control/dmp/__pycache__/__init__.cpython-310.pyc
+ur_simple_control/dmp/__pycache__/__init__.cpython-311.pyc
+ur_simple_control/dmp/__pycache__/__init__.cpython-312.pyc
+ur_simple_control/dmp/__pycache__/create_dmp.cpython-310.pyc
+ur_simple_control/dmp/__pycache__/dmp.cpython-310.pyc
+ur_simple_control/dmp/__pycache__/dmp.cpython-311.pyc
+ur_simple_control/dmp/__pycache__/dmp.cpython-312.pyc
+ur_simple_control/dmp/__pycache__/robotiq_gripper.cpython-310.pyc
+ur_simple_control/dmp/__pycache__/temporal_coupling.cpython-310.pyc
+ur_simple_control/dmp/trajectories/new_traj.csv
+ur_simple_control/dmp/trajectories/path_in_pixels.csv
+ur_simple_control/dmp/trajectories/ur10_omega_trajectory.csv
+ur_simple_control/robot_descriptions/__init__.py
+ur_simple_control/robot_descriptions/my_robot_calibration.yaml
+ur_simple_control/robot_descriptions/__pycache__/__init__.cpython-310.pyc
+ur_simple_control/robot_descriptions/__pycache__/__init__.cpython-311.pyc
+ur_simple_control/robot_descriptions/__pycache__/__init__.cpython-312.pyc
+ur_simple_control/robot_descriptions/config/__init__.py
+ur_simple_control/robot_descriptions/config/ur5e/__init__.py
+ur_simple_control/robot_descriptions/config/ur5e/default_kinematics.yaml
+ur_simple_control/robot_descriptions/config/ur5e/joint_limits.yaml
+ur_simple_control/robot_descriptions/config/ur5e/physical_parameters.yaml
+ur_simple_control/robot_descriptions/config/ur5e/visual_parameters.yaml
+ur_simple_control/robot_descriptions/meshes/__init__.py
+ur_simple_control/robot_descriptions/meshes/robotiq_85_coupler.stl
+ur_simple_control/robot_descriptions/meshes/ur5e/__init__.py
+ur_simple_control/robot_descriptions/meshes/ur5e/robotiq_85_coupler.stl
+ur_simple_control/robot_descriptions/meshes/ur5e/collision/__init__.py
+ur_simple_control/robot_descriptions/meshes/ur5e/collision/base.stl
+ur_simple_control/robot_descriptions/meshes/ur5e/collision/finger_1-collision.dae
+ur_simple_control/robot_descriptions/meshes/ur5e/collision/finger_2-collision.dae
+ur_simple_control/robot_descriptions/meshes/ur5e/collision/forearm.stl
+ur_simple_control/robot_descriptions/meshes/ur5e/collision/hand-e-collision.dae
+ur_simple_control/robot_descriptions/meshes/ur5e/collision/shoulder.stl
+ur_simple_control/robot_descriptions/meshes/ur5e/collision/upperarm.stl
+ur_simple_control/robot_descriptions/meshes/ur5e/collision/wrist1.stl
+ur_simple_control/robot_descriptions/meshes/ur5e/collision/wrist2.stl
+ur_simple_control/robot_descriptions/meshes/ur5e/collision/wrist3.stl
+ur_simple_control/robot_descriptions/meshes/ur5e/visual/__init__.py
+ur_simple_control/robot_descriptions/meshes/ur5e/visual/base.dae
+ur_simple_control/robot_descriptions/meshes/ur5e/visual/finger_1.dae
+ur_simple_control/robot_descriptions/meshes/ur5e/visual/finger_2.dae
+ur_simple_control/robot_descriptions/meshes/ur5e/visual/forearm.dae
+ur_simple_control/robot_descriptions/meshes/ur5e/visual/hand-e.dae
+ur_simple_control/robot_descriptions/meshes/ur5e/visual/shoulder.dae
+ur_simple_control/robot_descriptions/meshes/ur5e/visual/upperarm.dae
+ur_simple_control/robot_descriptions/meshes/ur5e/visual/wrist1.dae
+ur_simple_control/robot_descriptions/meshes/ur5e/visual/wrist2.dae
+ur_simple_control/robot_descriptions/meshes/ur5e/visual/wrist3.dae
+ur_simple_control/robot_descriptions/urdf/__init__.py
+ur_simple_control/robot_descriptions/urdf/ur5e_with_robotiq_hande.urdf
+ur_simple_control/robot_descriptions/urdf/ur5e_with_robotiq_hande_FIXED_PATHS.urdf
+ur_simple_control/robot_descriptions/urdf/__pycache__/__init__.cpython-310.pyc
+ur_simple_control/robot_descriptions/urdf/__pycache__/__init__.cpython-311.pyc
+ur_simple_control/robot_descriptions/urdf/__pycache__/__init__.cpython-312.pyc
+ur_simple_control/util/__init__.py
+ur_simple_control/util/calib_board_hacks.py
+ur_simple_control/util/default_args.py
+ur_simple_control/util/draw_path.py
+ur_simple_control/util/freedrive.py
+ur_simple_control/util/ft_calibration.py
+ur_simple_control/util/get_model.py
+ur_simple_control/util/logging_utils.py
+ur_simple_control/util/path_in_pixels.csv
+ur_simple_control/util/__pycache__/__init__.cpython-310.pyc
+ur_simple_control/util/__pycache__/__init__.cpython-311.pyc
+ur_simple_control/util/__pycache__/__init__.cpython-312.pyc
+ur_simple_control/util/__pycache__/boilerplate_wrapper.cpython-310.pyc
+ur_simple_control/util/__pycache__/calib_board_hacks.cpython-310.pyc
+ur_simple_control/util/__pycache__/calib_board_hacks.cpython-311.pyc
+ur_simple_control/util/__pycache__/calib_board_hacks.cpython-312.pyc
+ur_simple_control/util/__pycache__/draw_path.cpython-310.pyc
+ur_simple_control/util/__pycache__/draw_path.cpython-311.pyc
+ur_simple_control/util/__pycache__/draw_path.cpython-312.pyc
+ur_simple_control/util/__pycache__/freedrive.cpython-310.pyc
+ur_simple_control/util/__pycache__/freedrive.cpython-311.pyc
+ur_simple_control/util/__pycache__/freedrive.cpython-312.pyc
+ur_simple_control/util/__pycache__/get_model.cpython-310.pyc
+ur_simple_control/util/__pycache__/get_model.cpython-311.pyc
+ur_simple_control/util/__pycache__/get_model.cpython-312.pyc
+ur_simple_control/util/__pycache__/logging_utils.cpython-310.pyc
+ur_simple_control/util/__pycache__/logging_utils.cpython-311.pyc
+ur_simple_control/util/__pycache__/logging_utils.cpython-312.pyc
+ur_simple_control/util/__pycache__/robotiq_gripper.cpython-310.pyc
+ur_simple_control/util/__pycache__/robotiq_gripper.cpython-311.pyc
+ur_simple_control/util/__pycache__/robotiq_gripper.cpython-312.pyc
+ur_simple_control/visualize/__init__.py
+ur_simple_control/visualize/main.py
+ur_simple_control/visualize/make_run.py
+ur_simple_control/visualize/manipulator_visual_motion_analyzer.py
+ur_simple_control/visualize/visualize.py
+ur_simple_control/visualize/__pycache__/__init__.cpython-310.pyc
+ur_simple_control/visualize/__pycache__/__init__.cpython-311.pyc
+ur_simple_control/visualize/__pycache__/__init__.cpython-312.pyc
+ur_simple_control/visualize/__pycache__/make_run.cpython-310.pyc
+ur_simple_control/visualize/__pycache__/make_run.cpython-311.pyc
+ur_simple_control/visualize/__pycache__/make_run.cpython-312.pyc
+ur_simple_control/visualize/__pycache__/pin_to_vedo_plot_util.cpython-311.pyc
+ur_simple_control/visualize/__pycache__/visualize.cpython-310.pyc
+ur_simple_control/visualize/__pycache__/visualize.cpython-311.pyc
+ur_simple_control/visualize/__pycache__/visualize.cpython-312.pyc
+ur_simple_control/visualize/arms/another_debug_dh
+ur_simple_control/visualize/arms/j2n6s300_dh_params
+ur_simple_control/visualize/arms/j2s7s300_dh_params
+ur_simple_control/visualize/arms/kinova_jaco_params
+ur_simple_control/visualize/arms/kuka_lbw_iiwa_dh_params
+ur_simple_control/visualize/arms/lbr_iiwa_jos_jedan_dh
+ur_simple_control/visualize/arms/robot_parameters2
+ur_simple_control/visualize/arms/testing_dh_parameters
+ur_simple_control/visualize/arms/ur10e_dh_parameters_from_the_ur_site
+ur_simple_control/visualize/arms/ur5e_dh
+ur_simple_control/visualize/robot_stuff/InverseKinematics.py
+ur_simple_control/visualize/robot_stuff/drawing.py
+ur_simple_control/visualize/robot_stuff/drawing_for_anim.py
+ur_simple_control/visualize/robot_stuff/follow_curve.py
+ur_simple_control/visualize/robot_stuff/forw_kinm.py
+ur_simple_control/visualize/robot_stuff/inv_kinm.py
+ur_simple_control/visualize/robot_stuff/joint_as_hom_mat.py
+ur_simple_control/visualize/robot_stuff/utils.py
+ur_simple_control/visualize/robot_stuff/webots_api_helper_funs.py
+ur_simple_control/visualize/robot_stuff/__pycache__/InverseKinematics.cpython-310.pyc
+ur_simple_control/visualize/robot_stuff/__pycache__/InverseKinematics.cpython-311.pyc
+ur_simple_control/visualize/robot_stuff/__pycache__/InverseKinematics.cpython-312.pyc
+ur_simple_control/visualize/robot_stuff/__pycache__/drawing.cpython-310.pyc
+ur_simple_control/visualize/robot_stuff/__pycache__/drawing.cpython-311.pyc
+ur_simple_control/visualize/robot_stuff/__pycache__/drawing.cpython-312.pyc
+ur_simple_control/visualize/robot_stuff/__pycache__/drawing_for_anim.cpython-310.pyc
+ur_simple_control/visualize/robot_stuff/__pycache__/drawing_for_anim.cpython-311.pyc
+ur_simple_control/visualize/robot_stuff/__pycache__/drawing_for_anim.cpython-312.pyc
+ur_simple_control/visualize/robot_stuff/__pycache__/follow_curve.cpython-310.pyc
+ur_simple_control/visualize/robot_stuff/__pycache__/follow_curve.cpython-311.pyc
+ur_simple_control/visualize/robot_stuff/__pycache__/follow_curve.cpython-312.pyc
+ur_simple_control/visualize/robot_stuff/__pycache__/forw_kinm.cpython-310.pyc
+ur_simple_control/visualize/robot_stuff/__pycache__/forw_kinm.cpython-311.pyc
+ur_simple_control/visualize/robot_stuff/__pycache__/forw_kinm.cpython-312.pyc
+ur_simple_control/visualize/robot_stuff/__pycache__/inv_kinm.cpython-310.pyc
+ur_simple_control/visualize/robot_stuff/__pycache__/inv_kinm.cpython-311.pyc
+ur_simple_control/visualize/robot_stuff/__pycache__/inv_kinm.cpython-312.pyc
+ur_simple_control/visualize/robot_stuff/__pycache__/joint_as_hom_mat.cpython-310.pyc
+ur_simple_control/visualize/robot_stuff/__pycache__/joint_as_hom_mat.cpython-311.pyc
+ur_simple_control/visualize/robot_stuff/__pycache__/joint_as_hom_mat.cpython-312.pyc
+ur_simple_control/visualize/robot_stuff/__pycache__/utils.cpython-310.pyc
+ur_simple_control/visualize/robot_stuff/__pycache__/utils.cpython-311.pyc
+ur_simple_control/visualize/robot_stuff/__pycache__/utils.cpython-312.pyc
+ur_simple_control/visualize/robot_stuff/__pycache__/webots_api_helper_funs.cpython-310.pyc
+ur_simple_control/visualize/robot_stuff/__pycache__/webots_api_helper_funs.cpython-311.pyc
+ur_simple_control/visualize/robot_stuff/__pycache__/webots_api_helper_funs.cpython-312.pyc
\ No newline at end of file
diff --git a/python/ur_simple_control/basics/__pycache__/basics.cpython-312.pyc b/python/ur_simple_control/basics/__pycache__/basics.cpython-312.pyc
index 03f3f9a372b4970f16339d4e5aedf8edfc824b6a..a907dd1917993fe0c168898bc788a4aa7abd34c3 100644
Binary files a/python/ur_simple_control/basics/__pycache__/basics.cpython-312.pyc and b/python/ur_simple_control/basics/__pycache__/basics.cpython-312.pyc differ
diff --git a/python/ur_simple_control/basics/basics.py b/python/ur_simple_control/basics/basics.py
index 6b9f414014d44f5bc2e2f0e13977cc6ad689afd3..d81beb36ff7ec21e6520d05bfab108ca4915d6ed 100644
--- a/python/ur_simple_control/basics/basics.py
+++ b/python/ur_simple_control/basics/basics.py
@@ -10,14 +10,12 @@ from functools import partial
from ur_simple_control.managers import ControlLoopManager, RobotManager
import time
-"""
-moveJControlLoop
----------------
-generic control loop for clik (handling error to final point etc).
-in some version of the universe this could be extended to a generic
-point-to-point motion control loop.
-"""
def moveJControlLoop(q_desired, robot, i, past_data):
+ """
+ moveJControlLoop
+ ---------------
+ most basic P control for joint space point-to-point motion, actual control loop.
+ """
breakFlag = False
save_past_dict = {}
# you don't even need forward kinematics for this lmao
@@ -48,6 +46,12 @@ def moveJControlLoop(q_desired, robot, i, past_data):
# MOVEL works just fine, so apply whatever's missing for there here
# and that's it.
def moveJ(args, robot, q_desired):
+ """
+ moveJ
+ ---------------
+ most basic P control for joint space point-to-point motion.
+ just starts the control loop without any logging.
+ """
assert type(q_desired) == np.ndarray
controlLoop = partial(moveJControlLoop, q_desired, robot)
# we're not using any past data or logging, hence the empty arguments
@@ -57,3 +61,11 @@ def moveJ(args, robot, q_desired):
#time.sleep(0.01)
if args.debug_prints:
print("MoveJ done: convergence achieved, reached destionation!")
+
+
+def jointTrajFollowingPIDControlLoop():
+ pass
+
+
+def jointTrajFollowingPID():
+ pass
diff --git a/python/ur_simple_control/clik/__pycache__/clik_point_to_point.cpython-312.pyc b/python/ur_simple_control/clik/__pycache__/clik_point_to_point.cpython-312.pyc
index a300f6887915c8835819f530df29d28673262313..1d1e7ce98035d5c0df25852e9620c0fba4c5593c 100644
Binary files a/python/ur_simple_control/clik/__pycache__/clik_point_to_point.cpython-312.pyc and b/python/ur_simple_control/clik/__pycache__/clik_point_to_point.cpython-312.pyc differ
diff --git a/python/ur_simple_control/clik/clik_point_to_point.py b/python/ur_simple_control/clik/clik_point_to_point.py
index 514613736a71a71285d7aaa8b68468299ef04269..83ab327900f78d78898072c0ca5c86934b3b426d 100644
--- a/python/ur_simple_control/clik/clik_point_to_point.py
+++ b/python/ur_simple_control/clik/clik_point_to_point.py
@@ -7,22 +7,22 @@ from ur_simple_control.managers import ControlLoopManager, RobotManager
import time
from qpsolvers import solve_qp
-"""
-Every imaginable magic number, flag and setting is put in here.
-This way the rest of the code is clean.
-If you want to know what these various arguments do, just grep
-though the code to find them (but replace '-' with '_' in multi-word arguments).
-All the sane defaults are here, and you can change any number of them as an argument
-when running your program. If you want to change a drastic amount of them, and
-not have to run a super long commands, just copy-paste this function and put your
-own parameters as default ones.
-NOTE1: the args object obtained from args = parser.get_args()
-is pushed all around the rest of the code (because it's tidy), so don't change their names.
-NOTE2: that you need to copy-paste and add aguments you need
-to the script you will be writing. This is kept here
-only as a convenient reference point.
-"""
def get_args():
+ """
+ Every imaginable magic number, flag and setting is put in here.
+ This way the rest of the code is clean.
+ If you want to know what these various arguments do, just grep
+ though the code to find them (but replace '-' with '_' in multi-word arguments).
+ All the sane defaults are here, and you can change any number of them as an argument
+ when running your program. If you want to change a drastic amount of them, and
+ not have to run a super long commands, just copy-paste this function and put your
+ own parameters as default ones.
+ NOTE1: the args object obtained from args = parser.get_args()
+ is pushed all around the rest of the code (because it's tidy), so don't change their names.
+ NOTE2: that you need to copy-paste and add aguments you need
+ to the script you will be writing. This is kept here
+ only as a convenient reference point.
+ """
parser = argparse.ArgumentParser(description='Run closed loop inverse kinematics \
of various kinds. Make sure you know what the goal is before you run!',
formatter_class=argparse.ArgumentDefaultsHelpFormatter)
@@ -117,17 +117,17 @@ def invKinmQP(J, err_vector):
qd = solve_qp(P, q, G, h, A, b, lb, ub, solver="quadprog")
return qd
-"""
-getClikController
------------------
-A string argument is used to select one of these.
-It's a bit ugly, bit totally functional and OK solution.
-we want all of theme to accept the same arguments, i.e. the jacobian and the error vector.
-if they have extra stuff, just map it in the beginning with partial
-NOTE: this could be changed to something else if it proves inappropriate later
-TODO: write out other algorithms
-"""
def getClikController(args):
+ """
+ getClikController
+ -----------------
+ A string argument is used to select one of these.
+ It's a bit ugly, bit totally functional and OK solution.
+ we want all of theme to accept the same arguments, i.e. the jacobian and the error vector.
+ if they have extra stuff, just map it in the beginning with partial
+ NOTE: this could be changed to something else if it proves inappropriate later
+ TODO: write out other algorithms
+ """
if args.clik_controller == "dampedPseudoinverse":
return partial(dampedPseudoinverse, args.tikhonov_damp)
if args.clik_controller == "jacobianTranspose":
@@ -155,14 +155,14 @@ def getClikController(args):
# modularity yo
# past data to comply with the API
# TODO make this a kwarg or whatever to be more lax with this
-"""
-controlLoopClik
----------------
-generic control loop for clik (handling error to final point etc).
-in some version of the universe this could be extended to a generic
-point-to-point motion control loop.
-"""
def controlLoopClik(robot, clik_controller, i, past_data):
+ """
+ controlLoopClik
+ ---------------
+ generic control loop for clik (handling error to final point etc).
+ in some version of the universe this could be extended to a generic
+ point-to-point motion control loop.
+ """
breakFlag = False
log_item = {}
# know where you are, i.e. do forward kinematics
@@ -200,14 +200,14 @@ def controlLoopClik(robot, clik_controller, i, past_data):
return breakFlag, {}, log_item
-"""
-moveUntilContactControlLoop
----------------
-generic control loop for clik (handling error to final point etc).
-in some version of the universe this could be extended to a generic
-point-to-point motion control loop.
-"""
def moveUntilContactControlLoop(contact_detecting_force, speed, robot, clik_controller, i, past_data):
+ """
+ moveUntilContactControlLoop
+ ---------------
+ generic control loop for clik (handling error to final point etc).
+ in some version of the universe this could be extended to a generic
+ point-to-point motion control loop.
+ """
breakFlag = False
# know where you are, i.e. do forward kinematics
q = robot.getQ()
@@ -233,12 +233,12 @@ def moveUntilContactControlLoop(contact_detecting_force, speed, robot, clik_cont
robot.sendQd(qd)
return breakFlag, {}, {}
-"""
-moveUntilContact
------
-does clik until it feels something with the f/t sensor
-"""
def moveUntilContact(args, robot, speed):
+ """
+ moveUntilContact
+ -----
+ does clik until it feels something with the f/t sensor
+ """
assert type(speed) == np.ndarray
clik_controller = getClikController(args)
# TODO: just make the manager pass the robot or something, this is weird man
@@ -250,14 +250,14 @@ def moveUntilContact(args, robot, speed):
time.sleep(0.01)
print("Collision detected!!")
-"""
-moveL
------
-does moveL.
-send a SE3 object as goal point.
-if you don't care about rotation, make it np.zeros((3,3))
-"""
def moveL(args, robot, goal_point):
+ """
+ moveL
+ -----
+ does moveL.
+ send a SE3 object as goal point.
+ if you don't care about rotation, make it np.zeros((3,3))
+ """
assert type(goal_point) == pin.pinocchio_pywrap.SE3
robot.Mgoal = copy.deepcopy(goal_point)
clik_controller = getClikController(args)
@@ -274,14 +274,14 @@ def moveL(args, robot, goal_point):
print("MoveL done: convergence achieved, reached destionation!")
-"""
-controlLoopClik
----------------
-generic control loop for clik (handling error to final point etc).
-in some version of the universe this could be extended to a generic
-point-to-point motion control loop.
-"""
def controlLoopCompliantClik(args, robot : RobotManager, i, past_data):
+ """
+ controlLoopClik
+ ---------------
+ generic control loop for clik (handling error to final point etc).
+ in some version of the universe this could be extended to a generic
+ point-to-point motion control loop.
+ """
breakFlag = False
log_item = {}
save_past_dict = {}
@@ -328,16 +328,17 @@ def controlLoopCompliantClik(args, robot : RobotManager, i, past_data):
# we're not saving here, but need to respect the API,
# hence the empty dict
return breakFlag, save_past_dict, log_item
-"""
-compliantMoveL
------
-does compliantMoveL - a moveL, but with compliance achieved
-through f/t feedback
-send a SE3 object as goal point.
-if you don't care about rotation, make it np.zeros((3,3))
-"""
+
# add a threshold for the wrench
def compliantMoveL(args, robot, goal_point):
+ """
+ compliantMoveL
+ -----
+ does compliantMoveL - a moveL, but with compliance achieved
+ through f/t feedback
+ send a SE3 object as goal point.
+ if you don't care about rotation, make it np.zeros((3,3))
+ """
# assert type(goal_point) == pin.pinocchio_pywrap.SE3
robot.Mgoal = copy.deepcopy(goal_point)
clik_controller = getClikController(args)
diff --git a/python/ur_simple_control/clik/clik_trajectory_following.py b/python/ur_simple_control/clik/clik_trajectory_following.py
index 621be7dc9234cc8bc663b6f0d9c5bb9ccc6bf797..eae999f69fee0c5a9a82fc99adfac03976054ae1 100644
--- a/python/ur_simple_control/clik/clik_trajectory_following.py
+++ b/python/ur_simple_control/clik/clik_trajectory_following.py
@@ -14,23 +14,23 @@ from ur_simple_control.managers import ControlLoopManager
#######################################################################
# map the pixels to a 3D plane #
#######################################################################
-"""
-map2DPathTo3DPlane
---------------------
-TODO: THINK AND FINALIZE THE FRAME
-TODO: WRITE PRINT ABOUT THE FRAME TO THE USER
-assumptions:
-- origin in top-left corner (natual for western latin script writing)
-- x goes right (from TCP)
-- z will go away from the board
-- y just completes the right-hand frame
-TODO: RIGHT NOW we don't have a right-handed frame lmao, change that where it should be
-NOTE: this function as well be in the util or drawing file, but whatever for now, it will be done
- once it will actually be needed elsewhere
-Returns a 3D path appropriately scaled, and placed into the first quadrant
-of the x-y plane of the body-frame (TODO: what is the body frame if we're general?)
-"""
def map2DPathTo3DPlane(path_2D, width, height):
+ """
+ map2DPathTo3DPlane
+ --------------------
+ TODO: THINK AND FINALIZE THE FRAME
+ TODO: WRITE PRINT ABOUT THE FRAME TO THE USER
+ assumptions:
+ - origin in top-left corner (natual for western latin script writing)
+ - x goes right (from TCP)
+ - z will go away from the board
+ - y just completes the right-hand frame
+ TODO: RIGHT NOW we don't have a right-handed frame lmao, change that where it should be
+ NOTE: this function as well be in the util or drawing file, but whatever for now, it will be done
+ once it will actually be needed elsewhere
+ Returns a 3D path appropriately scaled, and placed into the first quadrant
+ of the x-y plane of the body-frame (TODO: what is the body frame if we're general?)
+ """
z = np.zeros((len(path_2D),1))
path_3D = np.hstack((path_2D,z))
# scale the path to m
@@ -43,43 +43,43 @@ def map2DPathTo3DPlane(path_2D, width, height):
return path_3D
-"""
-clikCartesianPathIntoJointPath
-------------------------------
-functionality
-------------
-Follows a provided Cartesian path,
-creates a joint space trajectory for it.
+def clikCartesianPathIntoJointPath(path, args, robot, \
+ """
+ clikCartesianPathIntoJointPath
+ ------------------------------
+ functionality
+ ------------
+ Follows a provided Cartesian path,
+ creates a joint space trajectory for it.
-return
-------
-- joint_space_trajectory to follow the given path.
+ return
+ ------
+ - joint_space_trajectory to follow the given path.
-arguments
-----------
-- path:
- --> cartesian path given in task frame
-TODO: write asserts for these arguments
-- args:
- --> all the magic numbers used here better be here
-- clikController:
- --> clik controller you're using
-- robot:
- --> RobotManager instance (needed for forward kinematics etc)
-TODO: maybe it's better design to expect path in body frame idk
- the good thing here is you're forced to think about frames.
-TODO: make this a kwarg with a neural transform as default.
-- transf_body_task_frame:
- --> A transformation from the body frame to task frame.
- --> It is assumed that the path was provided in the task frame
- because 9/10 times that's more convenient,
- and you can just pass a neural transform if you're not using it.
-- q_init:
- --> starting point.
- --> you can movej to it before executing the trajectory,
- so this makes perfect sense to ensure correctness
-"""
-def clikCartesianPathIntoJointPath(path, args, robot, \
+ arguments
+ ----------
+ - path:
+ --> cartesian path given in task frame
+ TODO: write asserts for these arguments
+ - args:
+ --> all the magic numbers used here better be here
+ - clikController:
+ --> clik controller you're using
+ - robot:
+ --> RobotManager instance (needed for forward kinematics etc)
+ TODO: maybe it's better design to expect path in body frame idk
+ the good thing here is you're forced to think about frames.
+ TODO: make this a kwarg with a neural transform as default.
+ - transf_body_task_frame:
+ --> A transformation from the body frame to task frame.
+ --> It is assumed that the path was provided in the task frame
+ because 9/10 times that's more convenient,
+ and you can just pass a neural transform if you're not using it.
+ - q_init:
+ --> starting point.
+ --> you can movej to it before executing the trajectory,
+ so this makes perfect sense to ensure correctness
+ """
clikController, q_init, R, p):
transf_body_to_task_frame = pin.SE3(R, p)
diff --git a/python/ur_simple_control/managers.py b/python/ur_simple_control/managers.py
index bdd49bb1703b2195177a47ef3bc27a6fec1175f0..9a7908a6066af4add6177116ecfa59f66034543e 100644
--- a/python/ur_simple_control/managers.py
+++ b/python/ur_simple_control/managers.py
@@ -13,7 +13,8 @@ import time
from rtde_control import RTDEControlInterface
from rtde_receive import RTDEReceiveInterface
from rtde_io import RTDEIOInterface
-from ur_simple_control.util.robotiq_gripper import RobotiqGripper
+from ur_simple_control.util.grippers.robotiq.robotiq_gripper import RobotiqGripper
+from ur_simple_control.util.grippers.on_robot.twofg import TWOFG
import copy
import signal
from ur_simple_control.util.get_model import get_model
@@ -57,49 +58,49 @@ There is an interface to a physics simulator.
Functions for torque controlled robots exist.
"""
-"""
-ControlLoopManager
--------------------
-Slightly fancier programming (passing a function as argument and using functools.partial)
-to get a wrapper around the control loop.
-In other words, it's the book-keeping around the actual control loop.
-It's a class because it keeps non-directly-control-loop-related parameters
-like max_iterations, what data to save etc.
-NOTE: you give this the ready-made control loop.
-if it has arguments, bake them in with functools.partial.
-Handles short-term data saving and logging.
-Details on this are given below.
-
-Short term data saving:
- - it's a dictionaries of deques (initialized here), because deque is the most convenient class
- for removing the first element and appending a last (it is just a linked list under the hood of course).
- - it's a dictionary for modularity's sake, because this way you can save whatever you want
- - and it will just work based on dictionary keys.
- - it is the user's resposibility to make sure they're providing correct data.
- - --> TODO but make an assert for the keys at least
- - in the c++ imlementation, make the user write their own struct or something.
- - since this is python, you need to give me initial values to infer types.
- - you need to provide initial values to populate the deque to start anyway.
-
-Logging data (for analysis and plotting):
- - it can only be handled here because the control loop itself only cares about the present/
- a small time-window around it.
- - saves it all in a dictionary of ndarrays (initialized here), returns that after a run
- TODO: it's provided by the user now, make it actually initialize here!!!
- - you need to specify which keys you're using to do the initialization
- - later, the controlLoop needs to return what's to be save in a small temporary dict.
- - NOTE: this is of course a somewhat redundant solution, but it's the simplest
- and most flexible way of doing it.
- it probably will be done some other way down the line, but it works and is not
- a priority right now
-
-Other info:
-- relies on RobotManager to handle all the magic numbers
- that are not truly only control loop related
+class ControlLoopManager:
+ """
+ ControlLoopManager
+ -------------------
+ Slightly fancier programming (passing a function as argument and using functools.partial)
+ to get a wrapper around the control loop.
+ In other words, it's the book-keeping around the actual control loop.
+ It's a class because it keeps non-directly-control-loop-related parameters
+ like max_iterations, what data to save etc.
+ NOTE: you give this the ready-made control loop.
+ if it has arguments, bake them in with functools.partial.
+ Handles short-term data saving and logging.
+ Details on this are given below.
+
+ Short term data saving:
+ - it's a dictionaries of deques (initialized here), because deque is the most convenient class
+ for removing the first element and appending a last (it is just a linked list under the hood of course).
+ - it's a dictionary for modularity's sake, because this way you can save whatever you want
+ - and it will just work based on dictionary keys.
+ - it is the user's resposibility to make sure they're providing correct data.
+ - --> TODO but make an assert for the keys at least
+ - in the c++ imlementation, make the user write their own struct or something.
+ - since this is python, you need to give me initial values to infer types.
+ - you need to provide initial values to populate the deque to start anyway.
+
+ Logging data (for analysis and plotting):
+ - it can only be handled here because the control loop itself only cares about the present/
+ a small time-window around it.
+ - saves it all in a dictionary of ndarrays (initialized here), returns that after a run
+ TODO: it's provided by the user now, make it actually initialize here!!!
+ - you need to specify which keys you're using to do the initialization
+ - later, the controlLoop needs to return what's to be save in a small temporary dict.
+ - NOTE: this is of course a somewhat redundant solution, but it's the simplest
+ and most flexible way of doing it.
+ it probably will be done some other way down the line, but it works and is not
+ a priority right now
+
+ Other info:
+ - relies on RobotManager to handle all the magic numbers
+ that are not truly only control loop related
+ """
-"""
-class ControlLoopManager:
def __init__(self, robot_manager, controlLoop, args, save_past_item, log_item):
signal.signal(signal.SIGINT, self.stopHandler)
self.max_iterations = args.max_iterations
@@ -145,15 +146,15 @@ class ControlLoopManager:
print("CONTROL_LOOP_MANAGER: i managed to put initializing log_item to queue")
- """
- run
- ---
- do timing to run at 500Hz.
- also handle the number of iterations.
- it's the controlLoop's responsibility to break if it achieved it's goals.
- this is done via the breakFlag
- """
def run(self):
+ """
+ run
+ ---
+ do timing to run at 500Hz.
+ also handle the number of iterations.
+ it's the controlLoop's responsibility to break if it achieved it's goals.
+ this is done via the breakFlag
+ """
self.final_iteration = 0
for i in range(self.max_iterations):
start = time.time()
@@ -242,16 +243,16 @@ class ControlLoopManager:
return self.log_dict, self.final_iteration
- """
- stopHandler
- -----------
- upon receiving SIGINT it sends zeros for speed commands to
- stop the robot.
- NOTE: apparently this isn't enough,
- nor does stopJ do anything, so it goes to freedriveMode
- and then exits it, which actually stops ur robots at least.
- """
def stopHandler(self, signum, frame):
+ """
+ stopHandler
+ -----------
+ upon receiving SIGINT it sends zeros for speed commands to
+ stop the robot.
+ NOTE: apparently this isn't enough,
+ nor does stopJ do anything, so it goes to freedriveMode
+ and then exits it, which actually stops ur robots at least.
+ """
print('sending 300 speedjs full of zeros and exiting')
for i in range(300):
vel_cmd = np.zeros(6)
@@ -305,30 +306,31 @@ class ControlLoopManager:
exit()
-"""
-robotmanager:
----------------
-- design goal: rely on pinocchio as much as possible while
- concealing obvious bookkeeping
-- right now it is assumed you're running this on UR5e so some
- magic numbers are just put to it.
- this will be extended once there's a need for it.
-- at this stage it's just a boilerplate reduction class
- but the idea is to inherit it for more complicated things
- with many steps, like dmp.
- or just showe additional things in, this is python after all
-- you write your controller separately,
- and then drop it into this - there is a wrapper function you put
- around the control loop which handles timing so that you
- actually run at 500Hz and not more.
-- this is probably not the most new-user friendly solution,
- but it's designed for fastest idea to implementation rate.
-- if this was a real programming language, all of these would really be private variables.
- as it currently stands, "private" functions have the '_' prefix
- while the public getters don't have a prefix.
-- TODO: write out default arguments needed here as well
-"""
class RobotManager:
+ """
+ RobotManager:
+ ---------------
+ - design goal: rely on pinocchio as much as possible while
+ concealing obvious bookkeeping
+ - right now it is assumed you're running this on UR5e so some
+ magic numbers are just put to it.
+ this will be extended once there's a need for it.
+ - at this stage it's just a boilerplate reduction class
+ but the idea is to inherit it for more complicated things
+ with many steps, like dmp.
+ or just showe additional things in, this is python after all
+ - you write your controller separately,
+ and then drop it into this - there is a wrapper function you put
+ around the control loop which handles timing so that you
+ actually run at 500Hz and not more.
+ - this is probably not the most new-user friendly solution,
+ but it's designed for fastest idea to implementation rate.
+ - if this was a real programming language, all of these would really be private variables.
+ as it currently stands, "private" functions have the '_' prefix
+ while the public getters don't have a prefix.
+ - TODO: write out default arguments needed here as well
+ """
+
# just pass all of the arguments here and store them as is
# so as to minimize the amount of lines.
# might be changed later if that seems more appropriate
@@ -336,7 +338,6 @@ class RobotManager:
self.args = args
self.pinocchio_only = args.pinocchio_only
self.simulation = args.simulation
- self.gripper_flag = args.gripper
# load model
# collision and visual models are none if args.visualize == False
self.model, self.collision_model, self.visual_model, self.data = \
@@ -391,10 +392,16 @@ class RobotManager:
# TODO: make general
- if self.gripper_flag and not self.pinocchio_only:
- self.gripper = RobotiqGripper()
- self.gripper.connect(args.robot_ip, 63352)
- self.gripper.activate()
+ self.gripper = None
+ if (self.args.gripper != "none") and not self.pinocchio_only:
+ if self.args.gripper == "robotiq":
+ self.gripper = RobotiqGripper()
+ self.gripper.connect(args.robot_ip, 63352)
+ self.gripper.activate()
+ if self.args.gripper == "onrobot":
+ device = OnRobotDevice()
+ device.getCB()
+ self.gripper = TWOFG(device)
# also TODO: figure out how to best solve the gripper_velocity problem
# NOTE: you need to initialize differently for other types of joints
@@ -419,7 +426,7 @@ class RobotManager:
# and you need to minus them to have usable readings.
# we provide this with calibrateFT
self.wrench_offset = self.calibrateFT()
- if self.gripper_flag:
+ if self.args.gripper == "robotiq":
self.gripper.connect("192.168.1.102", 63352)
# this is a blocking call
# this isn't actually needed and it's annoying
@@ -468,17 +475,17 @@ class RobotManager:
- """
- calibrateFT
- -----------
- Read from the f/t sensor a bit, average the results
- and return the result.
- This can be used to offset the bias of the f/t sensor.
- NOTE: this is not an ideal solution.
- ALSO TODO: test whether the offset changes when
- the manipulator is in different poses.
- """
def calibrateFT(self):
+ """
+ calibrateFT
+ -----------
+ Read from the f/t sensor a bit, average the results
+ and return the result.
+ This can be used to offset the bias of the f/t sensor.
+ NOTE: this is not an ideal solution.
+ ALSO TODO: test whether the offset changes when
+ the manipulator is in different poses.
+ """
ft_readings = []
print("Will read from f/t sensors for a some number of seconds")
print("and give you the average.")
@@ -504,28 +511,28 @@ class RobotManager:
print(self.wrench_offset)
return self.wrench_offset.copy()
- """
- _step
- ----
- - the idea is to update everything that should be updated
- on a step-by-step basis
- - the actual problem this is solving is that you're not calling
- forwardKinematics, an expensive call, more than once per step.
- - within the TODO is to make all (necessary) variable private
- so that you can rest assured that everything is handled the way
- it's supposed to be handled. then have getters for these
- private variables which return deepcopies of whatever you need.
- that way the computations done in the control loop
- can't mess up other things. this is important if you want
- to switch between controllers during operation and have a completely
- painless transition between them.
- TODO: make the getQ, getQd and the rest here do the actual communication,
- and make these functions private.
- then have the deepcopy getters public.
- also TODO: make ifs for the simulation etc.
- this is less ifs overall right.
- """
def _step(self):
+ """
+ _step
+ ----
+ - the idea is to update everything that should be updated
+ on a step-by-step basis
+ - the actual problem this is solving is that you're not calling
+ forwardKinematics, an expensive call, more than once per step.
+ - within the TODO is to make all (necessary) variable private
+ so that you can rest assured that everything is handled the way
+ it's supposed to be handled. then have getters for these
+ private variables which return deepcopies of whatever you need.
+ that way the computations done in the control loop
+ can't mess up other things. this is important if you want
+ to switch between controllers during operation and have a completely
+ painless transition between them.
+ TODO: make the getQ, getQd and the rest here do the actual communication,
+ and make these functions private.
+ then have the deepcopy getters public.
+ also TODO: make ifs for the simulation etc.
+ this is less ifs overall right.
+ """
self._getQ()
self._getQd()
#self._getWrench()
@@ -544,31 +551,31 @@ class RobotManager:
# TODO NOTE: you'll want to do the additional math for
# torque controlled robots here, but it's ok as is rn
- """
- setSpeedSlider
- ---------------
- update in all places
- """
def setSpeedSlider(self, value):
+ """
+ setSpeedSlider
+ ---------------
+ update in all places
+ """
assert value <= 1.0 and value > 0.0
if not self.args.pinocchio_only:
self.rtde_io.setSpeedSlider(value)
self.speed_slider = value
- """
- _getQ
- -----
- NOTE: private function for use in _step(), use the getter getQ()
- urdf treats gripper as two prismatic joints,
- but they do not affect the overall movement
- of the robot, so we add or remove 2 items to the joint list.
- also, the gripper is controlled separately so we'd need to do this somehow anyway
- NOTE: this gripper_past_pos thing is not working atm, but i'll keep it here as a TODO
- """
def _getQ(self):
+ """
+ _getQ
+ -----
+ NOTE: private function for use in _step(), use the getter getQ()
+ urdf treats gripper as two prismatic joints,
+ but they do not affect the overall movement
+ of the robot, so we add or remove 2 items to the joint list.
+ also, the gripper is controlled separately so we'd need to do this somehow anyway
+ NOTE: this gripper_past_pos thing is not working atm, but i'll keep it here as a TODO
+ """
if not self.pinocchio_only:
q = self.rtde_receive.getActualQ()
- if self.args.gripper:
+ if self.args.gripper == "robotiq":
# TODO: make it work or remove it
#self.gripper_past_pos = self.gripper_pos
# this is pointless by itself
@@ -587,19 +594,19 @@ class RobotManager:
q = np.array(q)
self.q = q
- """
- _getT_w_e
- -----
- NOTE: private function, use the getT_w_e() getter
- urdf treats gripper as two prismatic joints,
- but they do not affect the overall movement
- of the robot, so we add or remove 2 items to the joint list.
- also, the gripper is controlled separately so we'd need to do this somehow anyway
- NOTE: this gripper_past_pos thing is not working atm, but i'll keep it here as a TODO.
- NOTE: don't use this if use called _step() because it repeats forwardKinematics
- """
# TODO remove evil hack
def _getT_w_e(self, q_given=None):
+ """
+ _getT_w_e
+ -----
+ NOTE: private function, use the getT_w_e() getter
+ urdf treats gripper as two prismatic joints,
+ but they do not affect the overall movement
+ of the robot, so we add or remove 2 items to the joint list.
+ also, the gripper is controlled separately so we'd need to do this somehow anyway
+ NOTE: this gripper_past_pos thing is not working atm, but i'll keep it here as a TODO.
+ NOTE: don't use this if use called _step() because it repeats forwardKinematics
+ """
test = True
try:
test = q_given.all() == None
@@ -635,17 +642,17 @@ class RobotManager:
# TODO probably remove deepcopy
self.T_w_e = self.data.oMi[self.JOINT_ID]
- """
- _getQd
- -----
- NOTE: private function, use the _getQd() getter
- same note as _getQ.
- TODO NOTE: atm there's no way to get current gripper velocity.
- this means you'll probably want to read current positions and then finite-difference
- to get the velocity.
- as it stands right now, we'll just pass zeros in because I don't need this ATM
- """
def _getQd(self):
+ """
+ _getQd
+ -----
+ NOTE: private function, use the _getQd() getter
+ same note as _getQ.
+ TODO NOTE: atm there's no way to get current gripper velocity.
+ this means you'll probably want to read current positions and then finite-difference
+ to get the velocity.
+ as it stands right now, we'll just pass zeros in because I don't need this ATM
+ """
if not self.pinocchio_only:
qd = self.rtde_receive.getActualQd()
if self.args.gripper:
@@ -669,15 +676,15 @@ class RobotManager:
qd = np.array(qd)
self.v_q = qd
- """
- _getWrench
- -----
- different things need to be send depending on whether you're running a simulation,
- you're on a real robot, you're running some new simulator bla bla. this is handled
- here because this things depend on the arguments which are manager here (hence the
- class name RobotManager)
- """
def _getWrenchRaw(self):
+ """
+ _getWrench
+ -----
+ different things need to be send depending on whether you're running a simulation,
+ you're on a real robot, you're running some new simulator bla bla. this is handled
+ here because this things depend on the arguments which are manager here (hence the
+ class name RobotManager)
+ """
if not self.pinocchio_only:
wrench = np.array(self.rtde_receive.getActualTCPForce())
else:
@@ -706,15 +713,15 @@ class RobotManager:
mapping[3:6, 3:6] = self.T_w_e.rotation
self.wrench = mapping.T @ self.wrench
- """
- sendQd
- -----
- different things need to be send depending on whether you're running a simulation,
- you're on a real robot, you're running some new simulator bla bla. this is handled
- here because this things depend on the arguments which are manager here (hence the
- class name RobotManager)
- """
def sendQd(self, qd):
+ """
+ sendQd
+ -----
+ different things need to be send depending on whether you're running a simulation,
+ you're on a real robot, you're running some new simulator bla bla. this is handled
+ here because this things depend on the arguments which are manager here (hence the
+ class name RobotManager)
+ """
# we're hiding the extra 2 prismatic joint shenanigans from the control writer
# because there you shouldn't need to know this anyway
qd_cmd = qd[:6]
@@ -751,21 +758,21 @@ class RobotManager:
# utility functions #
#######################################################################
- """
- defineGoalPointCLI
- ------------------
- NOTE: this assume _step has not been called because it's run before the controlLoop
- --> best way to handle the goal is to tell the user where the gripper is
- in both UR tcp frame and with pinocchio and have them
- manually input it when running.
- this way you force the thinking before the moving,
- but you also get to view and analyze the information first
- TODO get the visual thing you did in ivc project with sliders also.
- it's just text input for now because it's totally usable, just not superb.
- but also you do want to have both options. obviously you go for the sliders
- in the case you're visualizing, makes no sense otherwise.
- """
def defineGoalPointCLI(self):
+ """
+ defineGoalPointCLI
+ ------------------
+ NOTE: this assume _step has not been called because it's run before the controlLoop
+ --> best way to handle the goal is to tell the user where the gripper is
+ in both UR tcp frame and with pinocchio and have them
+ manually input it when running.
+ this way you force the thinking before the moving,
+ but you also get to view and analyze the information first
+ TODO get the visual thing you did in ivc project with sliders also.
+ it's just text input for now because it's totally usable, just not superb.
+ but also you do want to have both options. obviously you go for the sliders
+ in the case you're visualizing, makes no sense otherwise.
+ """
self._getQ()
q = self.getQ()
# define goal
@@ -806,18 +813,18 @@ class RobotManager:
self.Mgoal = Mgoal
return Mgoal
- """
- killManipulatorVisualizer
- ---------------------------
- if you're using the manipulator visualizer, you want to start it only once.
- because you start the meshcat server, initialize the manipulator and then
- do any subsequent changes with that server. there's no point in restarting.
- but this means you have to kill it manually, because the ControlLoopManager
- can't nor should know whether this is the last control loop you're running -
- RobotManager has to handle the meshcat server.
- and in this case the user needs to say when the tasks are done.
- """
def killManipulatorVisualizer(self):
+ """
+ killManipulatorVisualizer
+ ---------------------------
+ if you're using the manipulator visualizer, you want to start it only once.
+ because you start the meshcat server, initialize the manipulator and then
+ do any subsequent changes with that server. there's no point in restarting.
+ but this means you have to kill it manually, because the ControlLoopManager
+ can't nor should know whether this is the last control loop you're running -
+ RobotManager has to handle the meshcat server.
+ and in this case the user needs to say when the tasks are done.
+ """
if self.args.debug_prints:
print("i am putting befree in plotter_queue to stop the manipulator visualizer")
# putting this command tells our process to kill the meshcat zmq server process
diff --git a/python/ur_simple_control/optimal_control/notes.md b/python/ur_simple_control/optimal_control/notes.md
new file mode 100644
index 0000000000000000000000000000000000000000..83489752a753b32e17450d09c79a0fe301bf164e
--- /dev/null
+++ b/python/ur_simple_control/optimal_control/notes.md
@@ -0,0 +1,8 @@
+## goal
+-----------
+use crocoddyl to compute a whole trajectory in advance.
+the trajectory can be followed with some other controller,
+with controls here being feed-forward or whatever
+
+
+
diff --git a/python/ur_simple_control/optimal_control/optimal_control.py b/python/ur_simple_control/optimal_control/optimal_control.py
new file mode 100644
index 0000000000000000000000000000000000000000..f5c724907b1a93c627849802192acfec8fb18c9c
--- /dev/null
+++ b/python/ur_simple_control/optimal_control/optimal_control.py
@@ -0,0 +1,7 @@
+import numpy as np
+import pinocchio as pin
+import crocoddyl
+from ur_simple_control.managers import ControlLoopManager, RobotManager
+
+def IKOCP(robot : RobotManager, goal : pin.SE3):
+ state = crocoddyl.StateMultibody(robot.model)
diff --git a/python/ur_simple_control/util/grippers/abstract_gripper.py b/python/ur_simple_control/util/grippers/abstract_gripper.py
new file mode 100644
index 0000000000000000000000000000000000000000..b8672d2969b9fe18aa594704c918e26f2e85d872
--- /dev/null
+++ b/python/ur_simple_control/util/grippers/abstract_gripper.py
@@ -0,0 +1,32 @@
+from abc import ABC
+
+class AbstractGripper(ABC):
+ """
+ AbstractGripper
+ ------------------
+ Abstract gripper class enforcing all grippers to have the same API toward RobotManager.
+ The point of this is to avoid having too many ifs in RobotManager
+ which reduce its readability, while achieving the same effect
+ of moving the stupid hardware bookkeeping out of sight.
+ Bookkeeping here refers to various grippers using different interfaces and differently
+ named functions just to do the same thing - move the gripper to a certain position
+ with a certain speed and certain final gripping force.
+ There are also the classic expected open(), close(), isGripping() quality-of-life functions.
+ """
+
+
+ def move(self, position, speed=None, gripping_force=None):
+ pass
+
+ def open(self):
+ pass
+
+ def close(self):
+ pass
+
+# def setVelocity(self):
+# pass
+#
+# def setGrippingForce(self):
+# pass
+#
diff --git a/python/ur_simple_control/util/grippers/on_robot/twofg.py b/python/ur_simple_control/util/grippers/on_robot/twofg.py
new file mode 100644
index 0000000000000000000000000000000000000000..e4b11514d6c111244f2a417deae674ab3a126d64
--- /dev/null
+++ b/python/ur_simple_control/util/grippers/on_robot/twofg.py
@@ -0,0 +1,281 @@
+import time
+import numpy as np
+import xmlrpc.client
+from ur_simple_control.util.grippers.abstract_gripper import AbstractGripper
+
+
+class OnRobotDevice:
+ '''
+ Generic OnRobot device object
+ '''
+ cb = None
+
+ def __init__(self, Global_cbip='192.168.1.1'):
+ #try to get Computebox IP address
+ try:
+ self.Global_cbip = Global_cbip
+ except NameError:
+ print("Global_cbip is not defined!")
+
+ def getCB(self):
+ try:
+ self.cb = xmlrpc.client.ServerProxy(
+ "http://" + str(self.Global_cbip) + ":41414/")
+ return self.cb
+ except TimeoutError:
+ print("Connection to ComputeBox failed!")
+
+
+'''
+XML-RPC library for controlling OnRobot devcies from Doosan robots
+
+Global_cbip holds the IP address of the compute box, needs to be defined by the end user
+'''
+
+
+class TWOFG(AbstractGripper):
+ '''
+ This class is for handling the 2FG device
+ '''
+ cb = None
+
+ def __init__(self, dev):
+ self.cb = dev.getCB()
+ # Device ID
+ self.TWOFG_ID = 0xC0
+
+ # Connection
+ self.CONN_ERR = -2 # Connection failure
+ self.RET_OK = 0 # Okay
+ self.RET_FAIL = -1 # Error
+
+ # arguments moved to these parameters because you almost certainly won't need this
+ self.t_index = 0
+ self.wait_for_grip = False
+ self.speed = 10 # [m/s]
+ self.gripping_force = 20 # [N]
+ self.max_width = self.get_max_exposition()
+ self.min_width = self.get_min_exposition()
+
+ def isConnected(self):
+ '''
+ Returns with True if 2FG device is connected, False otherwise
+
+ @param MOVED TO PROPERTY t_index: The position of the device (0 for single, 1 for dual primary, 2 for dual secondary)
+ @return: True if connected, False otherwise
+ @rtype: bool
+ '''
+ try:
+ IsTwoFG = self.cb.cb_is_device_connected(self.t_index, self.TWOFG_ID)
+ except TimeoutError:
+ IsTwoFG = False
+
+ if IsTwoFG is False:
+ print("No 2FG device connected on the given instance")
+ return False
+ else:
+ return True
+
+ def isBusy(self):
+ '''
+ Gets if the gripper is busy or not
+
+ @param MOVED TO PROPERTY t_index: The position of the device (0 for single, 1 for dual primary, 2 for dual secondary)
+ @type t_index: int
+
+ @rtype: bool
+ @return: True if busy, False otherwise
+ '''
+ if self.isConnected() is False:
+ return self.CONN_ERR
+ return self.cb.twofg_get_busy(self.t_index)
+
+ def setWaitForGrip(self, wait_for_grip : bool):
+ self.wait_for_grip = wait_for_grip
+
+ def open(self):
+ self.move(self.max_width)
+
+ def close(self):
+ self.move(self.min_width)
+
+ def setSpeed(speed : float):
+ self.speed = speed
+
+ def setGrippingForce(gripping_force : float):
+ self.gripping_force = gripping_force
+
+ def isGripped(self):
+ '''
+ Gets if the gripper is gripping or not
+
+ @param MOVED TO PROPERTY t_index: The position of the device (0 for single, 1 for dual primary, 2 for dual secondary)
+ @type t_index: int
+
+ @rtype: bool
+ @return: True if gripped, False otherwise
+ '''
+ if self.isConnected() is False:
+ return self.CONN_ERR
+ return self.cb.twofg_get_grip_detected()
+
+ def getStatus(self):
+ '''
+ Gets the status of the gripper
+
+ @param MOVED TO PROPERTY t_index: The position of the device (0 for single, 1 for dual primary, 2 for dual secondary)
+ @type t_index: int
+
+ @rtype: int
+ @return: Status code of the device
+ '''
+ if self.isConnected() is False:
+ return self.CONN_ERR
+ status = self.cb.twofg_get_status(self.t_index)
+ return status
+
+ def get_exposition(self):
+ '''
+ Returns with current external width
+
+ @param MOVE TO PROPERTY: t_index: The position of the device (0 for single, 1 for dual primary, 2 for dual secondary)
+ @return: External width in mm
+ @rtype: float
+ '''
+ if self.isConnected() is False:
+ return self.CONN_ERR
+ extWidth = self.cb.twofg_get_external_width(self.t_index)
+ return extWidth
+
+ def get_min_exposition(self):
+ '''
+ Returns with current minimum external width
+
+ @param MOVE TO PROPERTY: t_index: The position of the device (0 for single, 1 for dual primary, 2 for dual secondary)
+ @return: Minimum external width in mm
+ @rtype: float
+ '''
+ if self.isConnected() is False:
+ return self.CONN_ERR
+ extMinWidth = self.cb.twofg_get_min_external_width(self.t_index)
+ return extMinWidth
+
+ def get_max_exposition(self):
+ '''
+ Returns with current maximum external width
+
+ @param MOVED TO PROPERTY t_index: The position of the device (0 for single, 1 for dual primary, 2 for dual secondary)
+ @return: Maximum external width in mm
+ @rtype: float
+ '''
+ if self.isConnected() is False:
+ return self.CONN_ERR
+ extMaxWidth = self.cb.twofg_get_max_external_width(self.t_index)
+ return extMaxWidth
+
+ def get_force(self):
+ '''
+ Returns with current force
+
+ @param MOVED TO PROPERTY t_index: The position of the device (0 for single, 1 for dual primary, 2 for dual secondary)
+ @return: Force in N
+ @rtype: float
+ '''
+ if self.isConnected() is False:
+ return self.CONN_ERR
+ currForce = self.cb.twofg_get_force(self.t_index)
+ return currForce
+
+ def stop(self):
+ '''
+ Stop the grippers movement
+
+ @param MOVED TO PROPERTY t_index: The position of the device (0 for single, 1 for dual primary, 2 for dual secondary)
+ @type t_index: int
+ '''
+ if self.isConnected() is False:
+ return self.CONN_ERR
+ self.cb.twofg_stop(self.t_index)
+
+ def move(self, position, speed=None, gripping_force=None):
+ '''
+ Makes an external grip with the gripper to the desired position
+
+ @param MOVED TO PROPERTY t_index: The position of the device (0 for single, 1 for dual primary, 2 for dual secondary)
+ @param position: The width to move the gripper to in mm's
+ @type position: float
+ @param gripping_force: The force to move the gripper width in N
+ @type gripping_force: float
+ @param speed: The speed of the gripper in %
+ @type speed: int
+ @type self.wait_for_grip: bool
+ @param self.wait_for_grip: wait for the grip to end or not?
+ '''
+
+ if self.isConnected() is False:
+ return self.CONN_ERR
+
+ if speed == None:
+ speed = self.speed
+
+ if gripping_force == None:
+ gripping_force = self.gripping_force
+
+ #Sanity check
+ #self.max_width = self.get_max_exposition()
+ #self.min_width = self.get_min_exposition()
+ if position > self.max_width or position < self.min_width:
+ print("Invalid 2FG width parameter, " + str(self.max_width)+" - "+str(self.min_width) +" is valid only")
+ return self.RET_FAIL
+
+ if gripping_force > 140 or gripping_force < 20:
+ print("Invalid 2FG force parameter, 20-140 is valid only")
+ return self.RET_FAIL
+
+ if speed > 100 or speed < 10:
+ print("Invalid 2FG speed parameter, 10-100 is valid only")
+ return self.RET_FAIL
+
+ self.cb.twofg_grip_external(self.t_index, float(position), int(gripping_force), int(speed))
+
+ if self.wait_for_grip:
+ tim_cnt = 0
+ fbusy = self.isBusy()
+ while (fbusy):
+ time.sleep(0.1)
+ fbusy = self.isBusy()
+ tim_cnt += 1
+ if tim_cnt > 30:
+ print("2FG external grip command timeout")
+ break
+ else:
+ #Grip detection
+ grip_tim = 0
+ gripped = self.isGripped()
+ while (not gripped):
+ time.sleep(0.1)
+ gripped = self.isGripped()
+ grip_tim += 1
+ if grip_tim > 20:
+ print("2FG external grip detection timeout")
+ break
+ else:
+ return self.RET_OK
+ return self.RET_FAIL
+ return self.RET_FAIL
+ else:
+ return self.RET_OK
+
+
+if __name__ == '__main__':
+ device = OnRobotDevice()
+ device.getCB()
+ gripper_2FG7 = TWOFG(device)
+ #gripper_2FG7.grip(0, position=37.0)
+ #gripper_2FG7.grip(0, position=37.0)
+ gripper_2FG7.move(10.0)
+ time.sleep(1.0)
+ gripper_2FG7.close()
+ time.sleep(1.0)
+ gripper_2FG7.open()
+ print("Connection check: ", gripper_2FG7.isConnected())
diff --git a/python/ur_simple_control/util/robotiq_gripper.py b/python/ur_simple_control/util/grippers/robotiq/robotiq_gripper.py
similarity index 95%
rename from python/ur_simple_control/util/robotiq_gripper.py
rename to python/ur_simple_control/util/grippers/robotiq/robotiq_gripper.py
index 53efd879267a9ee28a58ecbebdcd008cc1450ac6..977db0c09042be89a0a6c348b36dcdcbcc33d2fb 100644
--- a/python/ur_simple_control/util/robotiq_gripper.py
+++ b/python/ur_simple_control/util/grippers/robotiq/robotiq_gripper.py
@@ -5,8 +5,10 @@ import threading
import time
from enum import Enum
from typing import Union, Tuple, OrderedDict
+from ur_simple_control.util.grippers.abstract_gripper import AbstractGripper
-class RobotiqGripper:
+
+class RobotiqGripper(AbstractGripper):
"""
Communicates with the gripper directly, via socket with string commands, leveraging string names for variables.
"""
@@ -281,13 +283,13 @@ class RobotiqGripper:
if log:
print(f"Gripper auto-calibrated to [{self.get_min_position()}, {self.get_max_position()}]")
- def move(self, position: int, speed: int, force: int) -> Tuple[bool, int]:
+ def move(self, position: int, speed=255, gripping_force=0) -> Tuple[bool, int]:
"""
Sends commands to start moving towards the given position, with the
- specified speed and force.
+ specified speed and gripping_force.
:param position: Position to move to [min_position, max_position]
:param speed: Speed to move at [min_speed, max_speed]
- :param force: Force to use [min_force, max_force]
+ :param gripping_force: Force to use [min_force, max_force]
:return: A tuple with a bool indicating whether the action it was successfully sent, and an integer with
the actual position that was requested, after being adjusted to the min/max calibrated range.
"""
@@ -297,12 +299,18 @@ class RobotiqGripper:
clip_pos = clip_val(self._min_position, position, self._max_position)
clip_spe = clip_val(self._min_speed, speed, self._max_speed)
- clip_for = clip_val(self._min_force, force, self._max_force)
+ clip_for = clip_val(self._min_force, gripping_force, self._max_force)
- # moves to the given position with the given speed and force
+ # moves to the given position with the given speed and gripping_force
var_dict = OrderedDict([(self.POS, clip_pos), (self.SPE, clip_spe), (self.FOR, clip_for), (self.GTO, 1)])
return self._set_vars(var_dict), clip_pos
+ def open(self):
+ self.move(0, 255, 255)
+
+ def close(self):
+ self.move(255, 255, 0)
+
def move_and_wait_for_pos(self, position: int, speed: int, force: int) -> Tuple[int, ObjectStatus]: # noqa
"""
Sends commands to start moving towards the given position, with the