diff --git a/convenience_tool_box/freedrive.py b/convenience_tool_box/freedrive.py
deleted file mode 100644
index d31cea5394a47c3f510c3f3b5ea31d71f240372a..0000000000000000000000000000000000000000
--- a/convenience_tool_box/freedrive.py
+++ /dev/null
@@ -1,31 +0,0 @@
-import numpy as np
-import time
-from rtde_control import RTDEControlInterface
-from rtde_receive import RTDEReceiveInterface
-from rtde_io import RTDEIOInterface
-import os
-import copy
-import signal
-
-def handler(signum, frame):
- print('i will end freedrive and exit')
- rtde_control.endFreedriveMode()
- exit()
-
-
-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
-print("connected")
-
-rtde_control.freedriveMode()
-signal.signal(signal.SIGINT, handler)
-
-while True:
- q = rtde_receive.getActualQ()
- q = np.array(q)
- print(*q.round(3), sep=', ')
- time.sleep(0.005)
diff --git a/convenience_tool_box/freedrive_v2.0.py b/convenience_tool_box/freedrive_v2.0.py
index 5c7c519fb3010e6e0f1782f1f1ecb9e8f6de7133..5577499de513dc50f4e6ea26078cda0200bf1ade 100644
--- a/convenience_tool_box/freedrive_v2.0.py
+++ b/convenience_tool_box/freedrive_v2.0.py
@@ -1,14 +1,5 @@
-import pinocchio as pin
-import numpy as np
-import time
-import argparse
-from functools import partial
-from ur_simple_control.managers import (
- getMinimalArgParser,
- ControlLoopManager,
- RobotManager,
-)
-from ur_simple_control.basics.basics import freedriveUntilKeyboard
+from smc import getMinimalArgParser, getRobotFromArgs
+from smc.control.freedrive import freedriveUntilKeyboard
import pickle
@@ -24,7 +15,7 @@ def get_args():
if __name__ == "__main__":
args = get_args()
- robot = RobotManager(args)
+ robot = getRobotFromArgs(args)
pose_n_q_dict = freedriveUntilKeyboard(args, robot)
file = open("./pose_n_q_dict.pickle", "wb")
@@ -33,15 +24,12 @@ if __name__ == "__main__":
print(pose_n_q_dict)
# get expected behaviour here (library can't know what the end is - you have to do this here)
- if not args.pinocchio_only:
+ if args.real:
robot.stopRobot()
if args.save_log:
- robot.log_manager.plotAllControlLoops()
+ robot._log_manager.saveLog()
+ robot._log_manager.plotAllControlLoops()
if args.visualizer:
robot.killManipulatorVisualizer()
-
- if args.save_log:
- robot.log_manager.saveLog()
- # loop_manager.stopHandler(None, None)
diff --git a/examples/cartesian_space/clik_point_to_point.py b/examples/cartesian_space/clik_point_to_point.py
index b1a8b8482befe3c34e1e258836247a7b141ee0b3..e0859528ee848e45b1b424eb8c0d7c0ee7ea8d9f 100644
--- a/examples/cartesian_space/clik_point_to_point.py
+++ b/examples/cartesian_space/clik_point_to_point.py
@@ -4,11 +4,10 @@ from smc.control.cartesian_space import getClikArgs
from smc.robots.utils import defineGoalPointCLI
from smc.control.cartesian_space.cartesian_space_point_to_point import moveL
-import numpy as np
import argparse
-def get_args():
+def get_args() -> argparse.Namespace:
parser = getMinimalArgParser()
parser.description = "Run closed loop inverse kinematics \
of various kinds. Make sure you know what the goal is before you run!"
@@ -17,7 +16,7 @@ def get_args():
"--randomly-generate-goal",
action=argparse.BooleanOptionalAction,
default=False,
- help="if true, rand generate a goal, if false you type it in via text input",
+ help="if true, the target pose is randomly generated, if false you type it target translation in via text input",
)
args = parser.parse_args()
return args
@@ -26,22 +25,20 @@ def get_args():
if __name__ == "__main__":
args = get_args()
robot = getRobotFromArgs(args)
- if args.randomly_generate_goal:
- Mgoal = getRandomlyGeneratedGoal(args)
+ if (not args.real) and args.randomly_generate_goal:
+ T_w_goal = getRandomlyGeneratedGoal(args)
if args.visualizer:
- robot.visualizer_manager.sendCommand({"Mgoal": Mgoal})
+ robot.visualizer_manager.sendCommand({"Mgoal": T_w_goal})
else:
- Mgoal = defineGoalPointCLI(robot)
- Mgoal.rotation = np.eye(3)
+ if args.real and args.randomly_generate_goal:
+ print("Ain't no way you're going to a random goal on the real robot!")
+ print("Look at the current pose, define something appropriate manually")
+ T_w_goal = defineGoalPointCLI(robot)
# compliantMoveL(args, robot, Mgoal)
- if robot.robot_name != "yumi":
- moveL(args, robot, Mgoal)
- # else:
- # goal_transform = pin.SE3.Identity()
- # goal_transform.translation[1] = 0.1
- # moveLDualArm(args, robot, Mgoal, goal_transform)
+ moveL(args, robot, T_w_goal)
robot.closeGripper()
robot.openGripper()
+
if args.real:
robot.stopRobot()
@@ -50,4 +47,3 @@ if __name__ == "__main__":
if args.save_log:
robot._log_manager.saveLog()
- # loop_manager.stopHandler(None, None)
diff --git a/examples/cartesian_space/yumi_self_collision.py b/examples/cartesian_space/yumi_self_collision.py
new file mode 100644
index 0000000000000000000000000000000000000000..f0c681ad0d121c605870dae1b0a188d513ea899a
--- /dev/null
+++ b/examples/cartesian_space/yumi_self_collision.py
@@ -0,0 +1,216 @@
+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+#
+# SPDX-License-Identifier: Apache-2.0
+# Copyright 2022 Stéphane Caron
+
+"""Yumi two-armed manipulator with definition of custom frame and
+collision avoidance w.r.t. those frames."""
+
+import meshcat_shapes
+import numpy as np
+import pinocchio as pin
+import qpsolvers
+from loop_rate_limiters import RateLimiter
+
+import pink
+from pink import solve_ik
+from pink.barriers import BodySphericalBarrier
+from pink.tasks import FrameTask, PostureTask
+from pink.visualization import start_meshcat_visualizer
+
+try:
+ from robot_descriptions.loaders.pinocchio import load_robot_description
+except ModuleNotFoundError as exc:
+ raise ModuleNotFoundError(
+ "Examples need robot_descriptions, "
+ "try `[conda|pip] install robot_descriptions`"
+ ) from exc # noqa: E501
+
+
+if __name__ == "__main__":
+ # Load the robot and define the custom frames
+ robot = load_robot_description("yumi_description", root_joint=None)
+
+ # Left arm frame
+ l_frame_placement = pin.SE3()
+ l_frame_placement.translation = np.array([0.0, 0.0, 0.05])
+ l_frame_placement.rotation = np.eye(3)
+
+ l_frame = pin.Frame(
+ "yumi_barrier_l",
+ robot.model.getJointId("yumi_joint_6_l"),
+ robot.model.getFrameId("yumi_link_7_l"),
+ l_frame_placement,
+ pin.FrameType.OP_FRAME,
+ )
+
+ robot.model.addFrame(l_frame)
+
+ # Right arm frame
+ r_frame_placement = pin.SE3()
+ r_frame_placement.translation = np.array([0.0, 0.0, 0.05])
+ r_frame_placement.rotation = np.eye(3)
+
+ r_frame = pin.Frame(
+ "yumi_barrier_r",
+ robot.model.getJointId("yumi_joint_6_r"),
+ robot.model.getFrameId("yumi_link_7_r"),
+ r_frame_placement,
+ pin.FrameType.OP_FRAME,
+ )
+
+ robot.model.addFrame(r_frame)
+ # Redefining robot data to take into account the new frames
+ robot.data = pin.Data(robot.model)
+
+ viz = start_meshcat_visualizer(robot)
+
+ # Pink tasks
+ left_end_effector_task = FrameTask(
+ "yumi_link_7_l",
+ position_cost=50.0, # [cost] / [m]
+ orientation_cost=10.0, # [cost] / [rad]
+ )
+ right_end_effector_task = FrameTask(
+ "yumi_link_7_r",
+ position_cost=50.0, # [cost] / [m]
+ orientation_cost=10.0, # [cost] / [rad]
+ )
+
+ # Pink barriers
+ ee_barrier = BodySphericalBarrier(
+ ("yumi_barrier_l", "yumi_barrier_r"),
+ d_min=0.25,
+ gain=100.0,
+ safe_displacement_gain=1.0,
+ )
+
+ posture_task = PostureTask(
+ cost=1e-3, # [cost] / [rad]
+ )
+
+ cbf_list = [ee_barrier]
+ tasks = [left_end_effector_task, right_end_effector_task, posture_task]
+
+ q_ref = np.array(
+ [
+ 0.045,
+ -0.155,
+ -0.394,
+ -0.617,
+ -0.939,
+ -0.343,
+ -1.216,
+ 0,
+ 0,
+ -0.374,
+ -0.249,
+ 0.562,
+ -0.520,
+ 0.934,
+ -0.337,
+ 1.400,
+ 0,
+ 0,
+ ]
+ )
+ configuration = pink.Configuration(robot.model, robot.data, q_ref)
+ for task in tasks:
+ task.set_target_from_configuration(configuration)
+ viz.display(configuration.q)
+
+ viewer = viz.viewer
+ meshcat_shapes.frame(viewer["left_end_effector"], opacity=1.0)
+ meshcat_shapes.sphere(
+ viewer["left_ee_barrier"],
+ opacity=0.4,
+ color=0xFF0000,
+ radius=0.125,
+ )
+ meshcat_shapes.sphere(
+ viewer["right_ee_barrier"],
+ opacity=0.4,
+ color=0x00FF00,
+ radius=0.125,
+ )
+ meshcat_shapes.frame(viewer["right_end_effector"], opacity=1.0)
+ meshcat_shapes.frame(viewer["left_end_effector_target"], opacity=1.0)
+ meshcat_shapes.frame(viewer["right_end_effector_target"], opacity=1.0)
+
+ # Select QP solver
+ solver = qpsolvers.available_solvers[0]
+ if "osqp" in qpsolvers.available_solvers:
+ solver = "osqp"
+
+ rate = RateLimiter(frequency=200.0)
+ dt = rate.period
+ t = 0.0 # [s]
+ l_y_des = np.array([0.392, 0.392, 0.6])
+ r_y_des = np.array([0.392, 0.392, 0.6])
+ l_dy_des = np.zeros(3)
+ r_dy_des = np.zeros(3)
+
+ while True:
+ # Calculate desired trajectory
+ A = 0.1
+ B = 0.1
+ # z -- 0.4 - 0.8
+ l_y_des[:] = (
+ 0.6,
+ 0.1 + B * np.sin(t),
+ 0.6 + A * np.sin(t),
+ )
+ r_y_des[:] = (
+ 0.6,
+ -0.1 - B * np.sin(t),
+ 0.6 + A * np.sin(t),
+ )
+ l_dy_des[:] = 0, B * np.cos(t), A * np.cos(t)
+ r_dy_des[:] = 0, -B * np.cos(t), A * np.cos(t)
+
+ left_end_effector_task.transform_target_to_world.translation = l_y_des
+ right_end_effector_task.transform_target_to_world.translation = r_y_des
+
+ # Update visualization frames
+ viewer["left_end_effector"].set_transform(
+ configuration.get_transform_frame_to_world(left_end_effector_task.frame).np
+ )
+ viewer["right_end_effector"].set_transform(
+ configuration.get_transform_frame_to_world(right_end_effector_task.frame).np
+ )
+ viewer["left_end_effector_target"].set_transform(
+ left_end_effector_task.transform_target_to_world.np
+ )
+ viewer["right_end_effector_target"].set_transform(
+ right_end_effector_task.transform_target_to_world.np
+ )
+
+ lb = configuration.get_transform_frame_to_world("yumi_barrier_l")
+ rb = configuration.get_transform_frame_to_world("yumi_barrier_r")
+
+ viewer["left_ee_barrier"].set_transform(lb.np)
+ viewer["right_ee_barrier"].set_transform(rb.np)
+
+ velocity = solve_ik(
+ configuration,
+ tasks,
+ dt,
+ solver=solver,
+ barriers=cbf_list,
+ )
+ configuration.integrate_inplace(velocity, dt)
+ dist_ee = (
+ np.linalg.norm(
+ configuration.get_transform_frame_to_world("yumi_barrier_l").translation
+ - configuration.get_transform_frame_to_world(
+ "yumi_barrier_r"
+ ).translation
+ )
+ * 100
+ )
+ print(f"Distance between end effectors: {dist_ee :0.1f}cm")
+ # Visualize result at fixed FPS
+ viz.display(configuration.q)
+ rate.sleep()
+ t += dt
diff --git a/examples/teaching_by_demonstration/naive_version.py b/examples/teaching_by_demonstration/naive_version.py
new file mode 100644
index 0000000000000000000000000000000000000000..77fae4418a2cd391a946ad7938f6dfc0c44888e9
--- /dev/null
+++ b/examples/teaching_by_demonstration/naive_version.py
@@ -0,0 +1,46 @@
+from smc import getMinimalArgParser, getRobotFromArgs
+from smc.control.freedrive import freedriveUntilKeyboard
+from smc.control.cartesian_space.cartesian_space_point_to_point import moveL
+from smc.control.cartesian_space import getClikArgs
+import pickle
+
+
+def get_args():
+ parser = getMinimalArgParser()
+ parser = getClikArgs(parser)
+ parser.description = (
+ "freedrive with keyboard bindings to save poses and joint angles"
+ )
+ # add more arguments here from different Simple Manipulator Control modules
+ args = parser.parse_args()
+ return args
+
+
+if __name__ == "__main__":
+ args = get_args()
+ robot = getRobotFromArgs(args)
+
+ try:
+ file = open("./pose_n_q_dict.pickle", "rb")
+ pose_n_q_dict = pickle.load(file)
+ file.close()
+ except FileNotFoundError:
+ pose_n_q_dict = freedriveUntilKeyboard(args, robot)
+ file = open("./pose_n_q_dict.pickle", "wb")
+ pickle.dump(pose_n_q_dict, file)
+ file.close()
+ print(pose_n_q_dict)
+
+ for T_w_goal in pose_n_q_dict["T_w_es"]:
+ moveL(args, robot, T_w_goal)
+
+ # get expected behaviour here (library can't know what the end is - you have to do this here)
+ if args.real:
+ robot.stopRobot()
+
+ if args.save_log:
+ robot._log_manager.saveLog()
+ robot._log_manager.plotAllControlLoops()
+
+ if args.visualizer:
+ robot.killManipulatorVisualizer()
diff --git a/python/smc/control/freedrive.py b/python/smc/control/freedrive.py
index 25c8194d5840945a1a10bf931034e4691110d455..4ebb0558d2d03b1c24bb4422556002c1092dc26d 100644
--- a/python/smc/control/freedrive.py
+++ b/python/smc/control/freedrive.py
@@ -7,6 +7,7 @@ import threading
from queue import Queue
from argparse import Namespace
from collections import deque
+from pinocchio import SE3
def freedriveControlLoop(
@@ -31,7 +32,7 @@ def freedriveControlLoop(
save_past_dict = {}
q = robot.q
- T_w_e = robot.T_w_e()
+ T_w_e = robot.T_w_e
if not com_queue.empty():
msg = com_queue.get()
@@ -51,7 +52,9 @@ def freedriveControlLoop(
return breakFlag, save_past_dict, log_item
-def freedriveUntilKeyboard(args, robot: SingleArmInterface):
+def freedriveUntilKeyboard(
+ args: Namespace, robot: SingleArmInterface
+) -> dict[str, list[SE3] | np.ndarray]:
"""
controlLoopFreedrive
-----------------------------
diff --git a/python/smc/robots/abstract_robotmanager.py b/python/smc/robots/abstract_robotmanager.py
index dea74f558e82f7d3ae62cfa1d095dba5f38eab92..6605d92b26b30d35d11275bd55a6a3c8df16cb40 100644
--- a/python/smc/robots/abstract_robotmanager.py
+++ b/python/smc/robots/abstract_robotmanager.py
@@ -66,7 +66,7 @@ class AbstractRobotManager(abc.ABC):
# and set this there.
self.gripper = None
# initialize things that depend on q here
- self._step()
+ # self._step()
####################################################################
# processes and utilities robotmanager owns #
diff --git a/python/smc/robots/implementations/ur5e.py b/python/smc/robots/implementations/ur5e.py
index ef7b7b832e2310a153bc289e7ac9003ec7e73ccc..6552d540ca9f2b7a08a291ea04185e095aacbf95 100644
--- a/python/smc/robots/implementations/ur5e.py
+++ b/python/smc/robots/implementations/ur5e.py
@@ -68,6 +68,13 @@ class RealUR5eRobotManager(AbstractUR5eRobotManager, AbstractRealRobotManager):
self._rtde_control: RTDEControlInterface
self._rtde_receive: RTDEReceiveInterface
self._rtde_io: RTDEIOInterface
+ # TODO: copy-pasted from ForceTorqueSensorInterface's __init__
+ # this should be inited automatically, it's not, todo is to make it work
+ self._wrench_base: np.ndarray = np.zeros(6)
+ self._wrench: np.ndarray = np.zeros(6)
+ # NOTE: wrench bias will be defined in the frame your sensor's gives readings
+ self._wrench_bias: np.ndarray = np.zeros(6)
+ self._T_w_e = pin.SE3.Identity()
super().__init__(args)
def connectToGripper(self):
@@ -113,11 +120,12 @@ class RealUR5eRobotManager(AbstractUR5eRobotManager, AbstractRealRobotManager):
self._rtde_control = RTDEControlInterface(self.args.robot_ip)
self._rtde_receive = RTDEReceiveInterface(self.args.robot_ip)
self._rtde_io = RTDEIOInterface(self.args.robot_ip)
- self._rtde_io.setSpeedSlider(self.args.speed_slider)
+ self._rtde_io.setSpeedSlider(self._speed_slider)
# NOTE: the force/torque sensor just has large offsets for no reason,
# and you need to minus them to have usable readings.
# we provide this with calibrateFT
- self.wrench_offset = self.calibrateFT(self._dt)
+ #self.wrench_offset = self.calibrateFT(self._dt)
+ self.calibrateFT(self._dt)
def setSpeedSlider(self, value):
"""
diff --git a/python/smc/robots/interfaces/force_torque_sensor_interface.py b/python/smc/robots/interfaces/force_torque_sensor_interface.py
index 1264e077dae4a8c5f87b5aa7744345919a8c2299..90c50655eae93ac58fec0fe9d144ef75a71249d3 100644
--- a/python/smc/robots/interfaces/force_torque_sensor_interface.py
+++ b/python/smc/robots/interfaces/force_torque_sensor_interface.py
@@ -8,7 +8,9 @@ import time
class ForceTorqueSensorInterface(abc.ABC):
def __init__(self):
# in base frame
- print("ForceTorqueSensorInterface init")
+ # TODO: update so that __init__ accepts args
+ # if args.debug_prints:
+ # print("ForceTorqueSensorInterface init")
self._wrench_base: np.ndarray = np.zeros(6)
self._wrench: np.ndarray = np.zeros(6)
# NOTE: wrench bias will be defined in the frame your sensor's gives readings
@@ -87,8 +89,9 @@ class ForceTorqueSensorInterface(abc.ABC):
self.zeroFtSensor()
for _ in range(2000):
start = time.time()
- ft = self._updateWrench()
- ft_readings.append(ft)
+ # ft = self._updateWrench()
+ self._updateWrench()
+ ft_readings.append(self._wrench_base.copy())
end = time.time()
diff = end - start
if diff < dt:
@@ -97,10 +100,15 @@ class ForceTorqueSensorInterface(abc.ABC):
ft_readings = np.array(ft_readings)
self._wrench_bias = np.average(ft_readings, axis=0)
print("The wrench bias is:", self._wrench_bias)
- return self._wrench_bias.copy()
class ForceTorqueOnSingleArmWrist(SingleArmInterface, ForceTorqueSensorInterface):
+ def __init__(self, args):
+ if args.debug_prints:
+ print("ForceTorqueOnSingleArmWrist init")
+ ForceTorqueSensorInterface.__init__(args)
+ super().__init__(args)
+
@property
def ft_sensor_frame_id(self) -> int:
return self._ee_frame_id
diff --git a/python/smc/robots/utils.py b/python/smc/robots/utils.py
index c53dda8f8f6047346006d905add20871b74320a8..faf7c5c4942d4af97cb1c2a1b4b4acc84e21e13d 100644
--- a/python/smc/robots/utils.py
+++ b/python/smc/robots/utils.py
@@ -196,9 +196,9 @@ def defineGoalPointCLI(robot):
in the case you're visualizing, makes no sense otherwise.
"""
robot._step()
- q = robot.q
+ robot._step()
# define goal
- T_w_e = robot.T_w_e
+ T_w_goal = robot.T_w_e
print("You can only specify the translation right now.")
if robot.args.real:
print(
@@ -212,13 +212,12 @@ def defineGoalPointCLI(robot):
*robot.data.oMi[6].translation.round(4),
*pin.rpy.matrixToRpy(robot.data.oMi[6].rotation).round(4),
)
- print("UR5e TCP:", *np.array(robot.rtde_receive.getActualTCPPose()).round(4))
+ print("UR5e TCP:", *np.array(robot._rtde_receive.getActualTCPPose()).round(4))
# remain with the current orientation
# TODO: add something, probably rpy for orientation because it's the least number
# of numbers you need to type in
- Mgoal = T_w_e.copy()
# this is a reasonable way to do it too, maybe implement it later
- # Mgoal.translation = Mgoal.translation + np.array([0.0, 0.0, -0.1])
+ # T_w_goal.translation = T_w_goal.translation + np.array([0.0, 0.0, -0.1])
# do a while loop until this is parsed correctly
while True:
goal = input(
@@ -234,17 +233,13 @@ def defineGoalPointCLI(robot):
print("The input is not in the expected format. Try again.")
print(e)
if e == "ok":
- Mgoal.translation = np.array(goal_list)
+ T_w_goal.translation = np.array(goal_list)
break
- print("this is goal pose you defined:\n", Mgoal)
+ print("this is goal pose you defined:\n", T_w_goal)
- # NOTE i'm not deepcopying this on purpose
- # but that might be the preferred thing, we'll see
- robot.Mgoal = Mgoal
if robot.args.visualizer:
- # TODO document this somewhere
- robot.visualizer_manager.sendCommand({"Mgoal": Mgoal})
- return Mgoal
+ robot.visualizer_manager.sendCommand({"Mgoal": T_w_goal})
+ return T_w_goal
# TODO: finish
diff --git a/python/smc/util/define_random_goal.py b/python/smc/util/define_random_goal.py
index 40ee89eff5d10429d6aaaa5290232b1b5c552ff2..364e082204e0bde093716ac076c9e381ba080592 100644
--- a/python/smc/util/define_random_goal.py
+++ b/python/smc/util/define_random_goal.py
@@ -1,14 +1,14 @@
-# PYTHON_ARGCOMPLETE_OK
import numpy as np
-import pinocchio as pin
+from pinocchio import SE3
+
def getRandomlyGeneratedGoal(args):
- Mgoal = pin.SE3.Random()
+ T_w_goal = SE3.Random()
# has to be close
- translation = np.random.random(3) * 0.4
+ translation = np.random.random(3) * 0.8 - 0.4
translation[2] = np.abs(translation[2])
translation = translation + np.ones(3) * 0.1
- Mgoal.translation = translation
+ T_w_goal.translation = translation
if args.debug_prints:
- print(Mgoal)
- return Mgoal
+ print(T_w_goal)
+ return T_w_goal