diff --git a/examples/navigation/mobile_base_navigation.py b/examples/navigation/mobile_base_navigation.py
new file mode 100644
index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391
diff --git a/python/smc/control/cartesian_space/cartesian_space_point_to_point.py b/python/smc/control/cartesian_space/cartesian_space_point_to_point.py
index aa84d9231c8567f25267d41c0104e079a828f7c2..945c100de8c9ed96dc960a2f9b6290669d20e892 100644
--- a/python/smc/control/cartesian_space/cartesian_space_point_to_point.py
+++ b/python/smc/control/cartesian_space/cartesian_space_point_to_point.py
@@ -28,7 +28,7 @@ def controlLoopClik(robot: SingleArmInterface, ik_solver, T_w_goal: pin.SE3, _,
err_vector = pin.log6(SEerror).vector
if np.linalg.norm(err_vector) < robot.args.goal_error:
breakFlag = True
- J = pin.computeFrameJacobian(robot.model, robot.data, q, robot.ee_frame_id)
+ J = robot.getJacobian()
# compute the joint velocities based on controller you passed
# qd = ik_solver(J, err_vector, past_qd=past_data['dqs_cmd'][-1])
qd = ik_solver(J, err_vector)
@@ -37,13 +37,13 @@ def controlLoopClik(robot: SingleArmInterface, ik_solver, T_w_goal: pin.SE3, _,
qd = dampedPseudoinverse(1e-2, J, err_vector)
robot.sendVelocityCommand(qd)
- log_item["qs"] = q.reshape((robot.model.nq,))
+ log_item["qs"] = q.reshape((robot.nq,))
log_item["dqs"] = robot.v
- log_item["dqs_cmd"] = qd.reshape((robot.model.nv,))
+ log_item["dqs_cmd"] = qd.reshape((robot.nv,))
log_item["err_norm"] = np.linalg.norm(err_vector).reshape((1,))
# we're not saving here, but need to respect the API,
# hence the empty dict
- save_past_item["dqs_cmd"] = qd.reshape((robot.model.nv,))
+ save_past_item["dqs_cmd"] = qd.reshape((robot.nv,))
return breakFlag, save_past_item, log_item
@@ -60,13 +60,13 @@ def moveL(args: Namespace, robot: SingleArmInterface, T_w_goal):
controlLoop = partial(controlLoopClik, robot, ik_solver, T_w_goal)
# we're not using any past data or logging, hence the empty arguments
log_item = {
- "qs": np.zeros(robot.model.nq),
- "dqs": np.zeros(robot.model.nv),
- "dqs_cmd": np.zeros(robot.model.nv),
+ "qs": np.zeros(robot.nq),
+ "dqs": np.zeros(robot.nv),
+ "dqs_cmd": np.zeros(robot.nv),
"err_norm": np.zeros(1),
}
save_past_dict = {
- "dqs_cmd": np.zeros(robot.model.nv),
+ "dqs_cmd": np.zeros(robot.nv),
}
loop_manager = ControlLoopManager(
robot, controlLoop, args, save_past_dict, log_item
@@ -125,16 +125,16 @@ def moveUntilContactControlLoop(
if np.linalg.norm(wrench[mask]) > args.contact_detecting_force:
print("hit with", np.linalg.norm(wrench[mask]))
breakFlag = True
- robot.sendVelocityCommand(np.zeros(robot.model.nq))
+ robot.sendVelocityCommand(np.zeros(robot.nq))
if (args.pinocchio_only) and (i > 500):
print("let's say you hit something lule")
breakFlag = True
# pin.computeJointJacobian is much different than the C++ version lel
- J = pin.computeFrameJacobian(robot.model, robot.data, q, robot.ee_frame_id)
+ J = robot.getJacobian()
# compute the joint velocities.
qd = ik_solver(J, speed)
robot.sendVelocityCommand(qd)
- log_item["qs"] = q.reshape((robot.model.nq,))
+ log_item["qs"] = q.reshape((robot.nq,))
log_item["wrench"] = wrench.reshape((6,))
return breakFlag, {}, log_item
@@ -152,7 +152,7 @@ def moveUntilContact(
controlLoop = partial(moveUntilContactControlLoop, args, robot, speed, ik_solver)
# we're not using any past data or logging, hence the empty arguments
log_item = {"wrench": np.zeros(6)}
- log_item["qs"] = np.zeros((robot.model.nq,))
+ log_item["qs"] = np.zeros((robot.nq,))
loop_manager = ControlLoopManager(robot, controlLoop, args, {}, log_item)
loop_manager.run()
print("Collision detected!!")
diff --git a/python/smc/control/control_loop_manager.py b/python/smc/control/control_loop_manager.py
index bf2c31a8359452353b672226c175ac2852e3327e..9ad9ef4557e9442ab7d1d9fcd9009b4124a0dc31 100644
--- a/python/smc/control/control_loop_manager.py
+++ b/python/smc/control/control_loop_manager.py
@@ -141,17 +141,21 @@ class ControlLoopManager:
if i % self.viz_update_rate == 0:
# don't send what wasn't ready
if self.args.visualizer:
+ self.robot_manager.visualizer_manager.sendCommand(
+ {
+ "q": self.robot_manager._q,
+ }
+ )
if self.robot_manager.robot_name != "yumi":
self.robot_manager.visualizer_manager.sendCommand(
{
- "q": self.robot_manager.q,
"T_w_e": self.robot_manager.T_w_e,
}
)
else:
T_w_e_left, T_w_e_right = self.robot_manager.T_w_e
self.robot_manager.visualizer_manager.sendCommand(
- {"q": self.robot_manager.q, "T_w_e": T_w_e_left}
+ {"T_w_e": T_w_e_left}
)
if self.robot_manager.robot_name == "heron":
T_base = self.robot_manager.data.oMi[1]
@@ -230,7 +234,7 @@ class ControlLoopManager:
# and put it into robot_manager.stopRobot()
print("sending 300 speedjs full of zeros and exiting")
for _ in range(300):
- vel_cmd = np.zeros(self.robot_manager.model.nv)
+ vel_cmd = np.zeros(self.robot_manager.nv)
self.robot_manager.sendVelocityCommand(vel_cmd)
self.robot_manager.stopRobot()
diff --git a/python/smc/robots/implementations/heron.py b/python/smc/robots/implementations/heron.py
index 9054445a1eb0e19eef3aee37177ba8ad3f2ade0e..99cd46ba0345c39f7f5fbafb34c2d6c199ef6ce0 100644
--- a/python/smc/robots/implementations/heron.py
+++ b/python/smc/robots/implementations/heron.py
@@ -3,6 +3,7 @@ from smc.robots.interfaces.force_torque_sensor_interface import (
ForceTorqueOnSingleArmWrist,
)
from smc.robots.interfaces.mobile_base_interface import MobileBaseInterface
+from smc.robots.interfaces.whole_body_interface import SingleArmWholeBodyInterface
from smc.robots.implementations.ur5e import get_model
import time
@@ -11,7 +12,9 @@ import pinocchio as pin
import hppfcl as fcl
-class RobotManagerHeron(ForceTorqueOnSingleArmWrist, MobileBaseInterface):
+class RobotManagerHeron(
+ ForceTorqueOnSingleArmWrist, SingleArmWholeBodyInterface, MobileBaseInterface
+):
@property
def model(self):
return self._model
@@ -34,6 +37,7 @@ class RobotManagerHeron(ForceTorqueOnSingleArmWrist, MobileBaseInterface):
self._model, self._collision_model, self._visual_model, self._data = (
heron_approximation()
)
+ self._mode = SingleArmWholeBodyInterface.control_mode.whole_body
self._ee_frame_id = self.model.getFrameId("tool0")
self._base_frame_id = self.model.getFrameId("mobile_base")
# TODO: CHANGE THIS TO REAL VALUES
diff --git a/python/smc/robots/interfaces/mobile_base_interface.py b/python/smc/robots/interfaces/mobile_base_interface.py
index e93a7a1ab2c2f634c11cbc9c37da1f12ee78693f..2593f6a63a522ae4aa8efde35457400f79abdcc9 100644
--- a/python/smc/robots/interfaces/mobile_base_interface.py
+++ b/python/smc/robots/interfaces/mobile_base_interface.py
@@ -1,7 +1,8 @@
-import numpy as np
+from smc.robots.robotmanager_abstract import AbstractRobotManager
+import numpy as np
import pinocchio as pin
-from smc.robots.robotmanager_abstract import AbstractRobotManager
+from argparse import Namespace
class MobileBaseInterface(AbstractRobotManager):
@@ -18,21 +19,13 @@ class MobileBaseInterface(AbstractRobotManager):
implementing an abstract mobile base interface.
"""
- def __init__(self, args):
+ def __init__(self, args: Namespace):
if args.debug_prints:
print("MobileBase init")
self._base_frame_id: int
self._T_w_b: pin.SE3
super().__init__(args)
- @property
- def base_position2D(self):
- return self._q[:2]
-
- @property
- def base_position3D(self):
- return np.array(list(self._q[:2]) + [0.0])
-
@property
def base_SE2_pose(self):
# NOTE: idk if this is the best way to calculate theta
diff --git a/python/smc/robots/interfaces/single_arm_interface.py b/python/smc/robots/interfaces/single_arm_interface.py
index b6f6e46f15f8a23eca52b0a75b836b2ef4e55b0d..ce49432eb3fa0bb3d43b6c27c91a40de78aa42a3 100644
--- a/python/smc/robots/interfaces/single_arm_interface.py
+++ b/python/smc/robots/interfaces/single_arm_interface.py
@@ -31,6 +31,11 @@ class SingleArmInterface(AbstractRobotManager):
pin.updateFramePlacement(self.model, self.data, self._ee_frame_id)
return self.data.oMf[self._ee_frame_id].copy()
+ def getJacobian(self) -> np.ndarray:
+ return pin.computeFrameJacobian(
+ self.model, self.data, self._q, self._ee_frame_id
+ )
+
def forwardKinematics(self):
pin.forwardKinematics(
self.model,
diff --git a/python/smc/robots/interfaces/whole_body_interface.py b/python/smc/robots/interfaces/whole_body_interface.py
new file mode 100644
index 0000000000000000000000000000000000000000..752fdff47f290a5b069ee8aec89fd726f1798926
--- /dev/null
+++ b/python/smc/robots/interfaces/whole_body_interface.py
@@ -0,0 +1,137 @@
+from smc.robots.interfaces.mobile_base_interface import MobileBaseInterface
+from smc.robots.interfaces.single_arm_interface import SingleArmInterface
+
+import pinocchio as pin
+from argparse import Namespace
+import numpy as np
+from enum import Enum
+
+# TODO: put back in when python3.12 will be safe to use
+# from typing import override
+
+
+class SingleArmWholeBodyInterface(SingleArmInterface, MobileBaseInterface):
+ """
+ SingleArmWholeBodyInterface
+ ---------------------------
+ exists to provide either:
+ 1) whole body values
+ 2) base only value
+ 3) arm only value
+
+ what you get depends on the mode you set - they're enumerate as above
+ """
+
+ control_mode = Enum("mode", [("whole_body", 1), ("base_only", 2), ("arm_only", 3)])
+
+ def __init__(self, args: Namespace):
+ if args.debug_prints:
+ print("SingleArmWholeBodyInterface init")
+ self._mode: SingleArmWholeBodyInterface.control_mode
+ super().__init__(args)
+
+ @property
+ def mode(self) -> control_mode:
+ return self._mode
+
+ @mode.setter
+ def setMode(self, mode: control_mode) -> None:
+ assert type(mode) is self.control_mode
+ self._mode = mode
+
+ # TODO: put back in when python3.12 will be safe to use
+ # @override
+ @property
+ def q(self) -> np.ndarray:
+ # 3if self._mode.value == 1:
+ # 3 return self._q.copy()
+
+ if self._mode.value == 2:
+ return self._q[:4]
+
+ if self._mode.value == 3:
+ return self._q[4:]
+
+ return self._q.copy()
+
+ @property
+ def nq(self):
+ if self._mode.value == 2:
+ return 4
+
+ if self._mode.value == 3:
+ return self.model.nq - 4
+
+ return self.model.nq
+
+ @property
+ def v(self) -> np.ndarray:
+ # 3if self._mode.value == 1:
+ # 3 return self._q.copy()
+
+ if self._mode.value == 2:
+ return self._v[:3]
+
+ if self._mode.value == 3:
+ return self._v[3:]
+
+ return self._v.copy()
+
+ @property
+ def nv(self):
+ if self._mode.value == 2:
+ return 3
+
+ if self._mode.value == 3:
+ return self.model.nv - 3
+
+ return self.model.nv
+
+ # TODO: put back in when python3.12 will be safe to use
+ # @override
+ def forwardKinematics(self) -> None:
+ pin.forwardKinematics(
+ self.model,
+ self.data,
+ self._q,
+ )
+ pin.updateFramePlacement(self.model, self.data, self._ee_frame_id)
+ pin.updateFramePlacement(self.model, self.data, self._base_frame_id)
+ self._T_w_e = self.data.oMf[self._ee_frame_id].copy()
+ self._T_w_b = self.data.oMf[self._base_frame_id].copy()
+
+ # TODO: put back in when python3.12 will be safe to use
+ # @override
+ def getJacobian(self) -> np.ndarray:
+ J_full = pin.computeFrameJacobian(
+ self.model, self.data, self._q, self._ee_frame_id
+ )
+ # if self._mode.value == 1:
+ # return J_full
+
+ if self._mode.value == 2:
+ return J_full[:, :3]
+
+ if self._mode.value == 3:
+ return J_full[:, 3:]
+
+ return J_full
+
+ # TODO: put back in when python3.12 will be safe to use
+ # @override
+ def sendVelocityCommand(self, v) -> None:
+ """
+ sendVelocityCommand
+ -------------------
+ 1) saturate the command to comply with hardware limits or smaller limits you've set
+ 2) send it via the particular robot's particular communication interface
+ """
+ if self._mode.value == 2:
+ v = np.hstack((v, np.zeros(self.model.nv - 3)))
+
+ if self._mode.value == 3:
+ v = np.hstack((np.zeros(3), v))
+ assert type(v) == np.ndarray
+ assert len(v) == self.model.nv
+ v = np.clip(v, -1 * self._max_v, self._max_v)
+ self.sendVelocityCommandToReal(v)
diff --git a/python/smc/robots/robotmanager_abstract.py b/python/smc/robots/robotmanager_abstract.py
index 2b94efd52e249f42b0b7c02c6a673fb423f01c26..ffe9976f1649a9c5392e89b4a991d55ecaeeec4f 100644
--- a/python/smc/robots/robotmanager_abstract.py
+++ b/python/smc/robots/robotmanager_abstract.py
@@ -108,8 +108,9 @@ class AbstractRobotManager(abc.ABC):
self.collision_model,
self.visual_model,
)
+ # NOTE: it HAS TO be _q, we override q to support control of subsets of joints
self.visualizer_manager = ProcessManager(
- args, side_function, {"q": self.q}, 0
+ args, side_function, {"q": self._q}, 0
)
# TODO: move this bs to visualizer, there are 0 reasons to keep it here
if args.visualize_collision_approximation:
@@ -141,10 +142,18 @@ class AbstractRobotManager(abc.ABC):
def q(self):
return self._q.copy()
+ @property
+ def nq(self):
+ return self.model.nq
+
@property
def v(self):
return self._v.copy()
+ @property
+ def nv(self):
+ return self.model.nv
+
# _updateQ and _updateV could be put into getters and setters of q and v respectively.
# however, because i don't trust the interfaces of a random robot to play ball,
# i want to keep this separated.
diff --git a/python/smc/robots/utils.py b/python/smc/robots/utils.py
index d18574145dd440bc8e8908e81c82d854697e37b2..c3720b626724c3e9d8fe9ff84506284db6195809 100644
--- a/python/smc/robots/utils.py
+++ b/python/smc/robots/utils.py
@@ -190,9 +190,7 @@ def defineGoalPointCLI(robot):
robot._step()
q = robot.q
# define goal
- pin.forwardKinematics(robot.model, robot.data, np.array(q))
- pin.updateFramePlacement(robot.model, robot.data, robot.ee_frame_id)
- T_w_e = robot.data.oMf[robot.ee_frame_id]
+ T_w_e = robot.T_w_e
print("You can only specify the translation right now.")
if robot.args.real:
print(