diff --git a/python/convenience_tool_box/freedrive.py b/python/convenience_tool_box/freedrive.py
index 307069d87d69fc612b04b50e621e975f746373b7..d31cea5394a47c3f510c3f3b5ea31d71f240372a 100644
--- a/python/convenience_tool_box/freedrive.py
+++ b/python/convenience_tool_box/freedrive.py
@@ -13,9 +13,9 @@ def handler(signum, frame):
exit()
-rtde_control = RTDEControlInterface("192.168.1.102")
-rtde_receive = RTDEReceiveInterface("192.168.1.102")
-rtde_io = RTDEIOInterface("192.168.1.102")
+rtde_control = RTDEControlInterface("192.168.1.103")
+rtde_receive = RTDEReceiveInterface("192.168.1.103")
+rtde_io = RTDEIOInterface("192.168.1.103")
rtde_io.setSpeedSlider(0.2)
while not rtde_control.isConnected():
continue
diff --git a/python/examples/clik.py b/python/examples/clik.py
index 622e719e90fb1756a205d959c659555605caadf5..85d9db2798046aaa44adece1b5bd1ae546de2462 100644
--- a/python/examples/clik.py
+++ b/python/examples/clik.py
@@ -54,6 +54,10 @@ def get_args():
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("--start-from-current-pose", action=argparse.BooleanOptionalAction, \
+ help="if connected to the robot, read the current pose and set it as the initial pose for the robot. \
+ very useful and convenient when running simulation before running on real", \
+ default=False)
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, \
@@ -91,6 +95,9 @@ if __name__ == "__main__":
robot = RobotManager(args)
Mgoal = robot.defineGoalPointCLI()
log_dict, final_iteration = compliantMoveL(args, robot, Mgoal)
+ robot.closeGripper()
+ time.sleep(2.0)
+ robot.openGripper()
if not args.pinocchio_only:
print("stopping via freedrive lel")
robot.rtde_control.freedriveMode()
diff --git a/python/examples/drawing_from_input_drawing.py b/python/examples/drawing_from_input_drawing.py
index 7c30f64ff7502ca661f7b37d898a17d8a1815f38..b9cbfb743d2c640ed04d28992acd1bf94197ad1e 100644
--- a/python/examples/drawing_from_input_drawing.py
+++ b/python/examples/drawing_from_input_drawing.py
@@ -77,6 +77,10 @@ def getArgs():
parser.add_argument('--goal-error', type=float, \
help="the final position error you are happy with. NOTE: not used here", \
default=1e-3)
+ parser.add_argument("--start-from-current-pose", action=argparse.BooleanOptionalAction, \
+ help="if connected to the robot, read the current pose and set it as the initial pose for the robot.
+ very useful and convenient when running simulation before running on real", \
+ default=False)
parser.add_argument('--tikhonov-damp', type=float, \
help="damping scalar in tikhonov regularization.\
This is used when generating the joint trajectory from the drawing.", \
diff --git a/python/examples/point_impedance_control.py b/python/examples/point_impedance_control.py
index 8167bcc184d9bd23c8bbdb040cb87fcdea60a5f5..fb58d87f764c76ce6b7f5f9a809be5a2e16f56ac 100644
--- a/python/examples/point_impedance_control.py
+++ b/python/examples/point_impedance_control.py
@@ -71,6 +71,10 @@ def getArgs():
help="the final position error you are happy with. NOTE: not used here", \
default=1e-3)
# TODO: test the interaction of this and the overall demo
+ parser.add_argument("--start-from-current-pose", action=argparse.BooleanOptionalAction, \
+ help="if connected to the robot, read the current pose and set it as the initial pose for the robot.
+ very useful and convenient when running simulation before running on real", \
+ default=False)
parser.add_argument('--tikhonov-damp', type=float, \
help="damping scalar in tikhonov regularization.\
This is used when generating the joint trajectory from the drawing.", \
diff --git a/python/ur_simple_control/__pycache__/managers.cpython-312.pyc b/python/ur_simple_control/__pycache__/managers.cpython-312.pyc
index b114db404caacd1a1e229e1b371bca3af042ebdc..b7c269db67426979fe7f362c0ef01a1ff2715e03 100644
Binary files a/python/ur_simple_control/__pycache__/managers.cpython-312.pyc and b/python/ur_simple_control/__pycache__/managers.cpython-312.pyc differ
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 1d1e7ce98035d5c0df25852e9620c0fba4c5593c..5bd4dd01b398cc1c6cba5665ae81632857b44b32 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/managers.py b/python/ur_simple_control/managers.py
index c6df9934788ecb644865673882c5fb9756ca3f09..824920001c76b7f3fe8df5dececb1087bf7cf2c4 100644
--- a/python/ur_simple_control/managers.py
+++ b/python/ur_simple_control/managers.py
@@ -130,19 +130,19 @@ class ControlLoopManager:
if self.args.real_time_plotting:
self.plotter_queue = Queue()
- if args.debug_prints:
+ if self.args.debug_prints:
print("CONTROL_LOOP_MANAGER", self.controlLoop, ": i created queue for real time plotting:", self.plotter_queue)
print("CONTROL_LOOP_MANAGER: i am creating and starting the real-time-plotter process")
self.real_time_plotter_process = Process(target=realTimePlotter,
args=(self.args, self.plotter_queue, ))
# give real-time plotter some time to set itself up
self.real_time_plotter_process.start()
- if args.debug_prints:
+ if self.args.debug_prints:
print("CONTROL_LOOP_MANAGER: real_time_plotter_process started")
# wait for feedback that the thing has started
self.plotter_queue.get()
self.plotter_queue.put(log_item)
- if args.debug_prints:
+ if self.args.debug_prints:
print("CONTROL_LOOP_MANAGER: i managed to put initializing log_item to queue")
@@ -390,8 +390,6 @@ class RobotManager:
# which is clipped, resulting in unexpected movement.
self.max_qd = 0.5 * args.speed_slider
-
- # TODO: make general
self.gripper = None
if (self.args.gripper != "none") and not self.pinocchio_only:
if self.args.gripper == "robotiq":
@@ -424,12 +422,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.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
-# # TODO: test after rebooting the robot
-# self.gripper.activate()
+
elif not args.pinocchio_only:
self.rtde_control = RTDEControlInterface("127.0.0.1")
self.rtde_receive = RTDEReceiveInterface("127.0.0.1")
@@ -439,6 +432,17 @@ class RobotManager:
if not args.pinocchio_only:
self.rtde_io.setSpeedSlider(args.speed_slider)
+ # TODO: make general for other robots
+ if args.pinocchio_only and args.start_from_current_pose:
+ self.rtde_receive = RTDEReceiveInterface(args.robot_ip)
+ q = self.rtde_receive.getActualQ()
+ q.append(0.0)
+ q.append(0.0)
+ q = np.array(q)
+ self.q = q
+ self.manipulator_visualizer_queue.put(q)
+
+
# do it once to get T_w_e
self._step()
@@ -742,12 +746,20 @@ class RobotManager:
self.q = pin.integrate(self.model, self.q, qd * self.dt)
def openGripper(self):
+ if self.gripper is None:
+ if self.args.debug_prints:
+ print("you didn't select a gripper (no gripper is the default parameter) so no gripping for you")
+ pass
if (not self.args.simulation) and (not self.args.pinocchio_only):
self.gripper.open()
else:
print("not implemented yet, so nothing is going to happen!")
def closeGripper(self):
+ if self.gripper is None:
+ if self.args.debug_prints:
+ print("you didn't select a gripper (no gripper is the default parameter) so no gripping for you")
+ pass
if (not self.args.simulation) and (not self.args.pinocchio_only):
self.gripper.close()
else:
@@ -824,11 +836,11 @@ class RobotManager:
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
- self.manipulator_visualizer_queue.put_nowait("befree")
- time.sleep(0.1)
- self.manipulator_visualizer_process.terminate()
- if self.args.debug_prints:
- print("terminated manipulator_visualizer_process")
+ 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
+ self.manipulator_visualizer_queue.put_nowait("befree")
+ time.sleep(0.1)
+ self.manipulator_visualizer_process.terminate()
+ if self.args.debug_prints:
+ print("terminated manipulator_visualizer_process")
diff --git a/python/ur_simple_control/util/grippers/on_robot/twofg.py b/python/ur_simple_control/util/grippers/on_robot/twofg.py
index e4b11514d6c111244f2a417deae674ab3a126d64..f6f665f819084a5431b7ae6af16b16a1200a9943 100644
--- a/python/ur_simple_control/util/grippers/on_robot/twofg.py
+++ b/python/ur_simple_control/util/grippers/on_robot/twofg.py
@@ -39,7 +39,8 @@ class TWOFG(AbstractGripper):
'''
cb = None
- def __init__(self, dev):
+ def __init__(self):
+ dev = OnRobotDevice()
self.cb = dev.getCB()
# Device ID
self.TWOFG_ID = 0xC0