diff --git a/python/examples/clik.py b/python/examples/clik.py
index cd07a47a12b4aceccabd3e4c071cb4b41f90fe5c..be65bc01e0deb01b0186eb9f0ad67c13c69c99cb 100644
--- a/python/examples/clik.py
+++ b/python/examples/clik.py
@@ -23,6 +23,7 @@ if __name__ == "__main__":
print(robot.getT_w_e())
Mgoal = robot.defineGoalPointCLI()
compliantMoveL(args, robot, Mgoal)
+ #moveL(args, robot, Mgoal)
robot.closeGripper()
robot.openGripper()
if not args.pinocchio_only:
diff --git a/python/examples/path_following_mpc.py b/python/examples/path_following_mpc.py
index 19dd8032ea65be3012ede34278e8c40cce9aac83..8afaf5703d3ee43b8cc1d1bcdd647c990ee26a53 100644
--- a/python/examples/path_following_mpc.py
+++ b/python/examples/path_following_mpc.py
@@ -30,6 +30,7 @@ if __name__ == "__main__":
if importlib.util.find_spec('mim_solvers'):
import mim_solvers
robot = RobotManager(args)
+ time.sleep(5)
# TODO: put this back for nicer demos
#Mgoal = robot.defineGoalPointCLI()
#Mgoal = pin.SE3.Random()
diff --git a/python/ur_simple_control/__pycache__/managers.cpython-312.pyc b/python/ur_simple_control/__pycache__/managers.cpython-312.pyc
index 8aac4dcf219d27bcd32fee4d77ef29b50de1926c..0fbef10f71ac938cc681cd484d7d87e0a2dd77e8 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/clik.py b/python/ur_simple_control/clik/clik.py
index d402bf8a3603cb0f71e52f26ef2d71ebe89e279c..3f39f815e75df421ff5d4690fd7c7078ef2f8530 100644
--- a/python/ur_simple_control/clik/clik.py
+++ b/python/ur_simple_control/clik/clik.py
@@ -160,7 +160,7 @@ def controlLoopClik(robot : RobotManager, clik_controller, i, past_data):
robot.sendQd(qd)
log_item['qs'] = q.reshape((robot.model.nq,))
- log_item['dqs'] = robot.getQd().reshape((robot.model.nq,))
+ log_item['dqs'] = robot.getQd().reshape((robot.model.nv,))
# we're not saving here, but need to respect the API,
# hence the empty dict
return breakFlag, {}, log_item
@@ -237,7 +237,7 @@ def moveL(args, robot : RobotManager, goal_point):
# 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.nq),
+ 'dqs' : np.zeros(robot.model.nv),
}
loop_manager = ControlLoopManager(robot, controlLoop, args, {}, log_item)
loop_manager.run()
diff --git a/python/ur_simple_control/managers.py b/python/ur_simple_control/managers.py
index df84f3b78a6b1eed963d92ba0d36b78d61c7811b..b6be29c9f186d8f31cc1a4f9c033947a1c8edfab 100644
--- a/python/ur_simple_control/managers.py
+++ b/python/ur_simple_control/managers.py
@@ -13,7 +13,7 @@ from ur_simple_control.util.grippers.robotiq.robotiq_gripper import RobotiqGripp
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, heron_approximation, getGripperlessUR5e
+from ur_simple_control.util.get_model import get_model, heron_approximation, heron_approximationDD, getGripperlessUR5e
from collections import deque
from ur_simple_control.visualize.visualize import plotFromDict, realTimePlotter, manipulatorVisualizer
from ur_simple_control.util.logging_utils import LogManager
@@ -79,7 +79,7 @@ def getMinimalArgParser():
#################################################
parser.add_argument('--robot', type=str, \
help="which robot you're running or simulating", default="ur5e", \
- choices=['ur5e', 'heron', 'gripperlessur5e'])
+ choices=['ur5e', 'heron', 'herondd', 'gripperlessur5e'])
parser.add_argument('--simulation', action=argparse.BooleanOptionalAction, \
help="whether you are running the UR simulator", default=False)
parser.add_argument('--robot-ip', type=str,
@@ -406,6 +406,9 @@ class RobotManager:
if self.robot_name == "heron":
self.model, self.collision_model, self.visual_model, self.data = \
heron_approximation()
+ if self.robot_name == "herondd":
+ self.model, self.collision_model, self.visual_model, self.data = \
+ heron_approximationDD()
if self.robot_name == "gripperlessur5e":
self.model, self.collision_model, self.visual_model, self.data = \
getGripperlessUR5e()
@@ -817,6 +820,9 @@ class RobotManager:
if self.robot_name == "heron":
self.v_q = qd
self.q = pin.integrate(self.model, self.q, qd * self.dt)
+ if self.robot_name == "herondd":
+ self.v_q = qd
+ self.q = pin.integrate(self.model, self.q, qd * self.dt)
if self.robot_name == "gripperlessur5e":
qd_cmd = qd[:6]
diff --git a/python/ur_simple_control/optimal_control/crocoddyl_mpc.py b/python/ur_simple_control/optimal_control/crocoddyl_mpc.py
index 2d36c726052bb28e40821619279bedc6d3ca060f..940acd6e580c4e344c5f6b8b81d5421f88d3a7b3 100644
--- a/python/ur_simple_control/optimal_control/crocoddyl_mpc.py
+++ b/python/ur_simple_control/optimal_control/crocoddyl_mpc.py
@@ -111,15 +111,23 @@ def CrocoPathFollowingMPCControlLoop(args, robot : RobotManager, solver : crocod
# TODO: implement
#path = robot.getPath()
+ # 1) make 6D path out of [[x0,y0],...]
+ # that represents the center of the cart
+ # 2) do T_mobile_base_ee_pos to get
+ # end-effector reference frame(s)
+
# generate bullshit just to see it works
T_w_e = robot.getT_w_e()
- translation = np.array([0.01, 0.0, 0.0])
path = []
+ t = i * robot.dt
for i in range(args.n_knots):
+ t += 0.01
new = T_w_e.copy()
- new.translation = new.translation + i * translation
+ translation = 2 * np.array([np.cos(t/20), np.sin(t/20), 0.3])
+ new.translation = translation
path.append(new)
+
x0 = np.concatenate([robot.getQ(), robot.getQd()])
solver.problem.x0 = x0
# warmstart solver with previous solution
diff --git a/python/ur_simple_control/util/__pycache__/get_model.cpython-312.pyc b/python/ur_simple_control/util/__pycache__/get_model.cpython-312.pyc
index 05d82360395cf608e6ad774a2bfb5c43c0ebd23f..361d94f551ed5213c11d66ae7a950361e4d7f57f 100644
Binary files a/python/ur_simple_control/util/__pycache__/get_model.cpython-312.pyc and b/python/ur_simple_control/util/__pycache__/get_model.cpython-312.pyc differ
diff --git a/python/ur_simple_control/util/get_model.py b/python/ur_simple_control/util/get_model.py
index afe4dcc7af341645d0e5c74c0bfae4d1cb394dc7..0a2f07581c7c4f6639fc45bcc5ec05a22fb9b7e8 100644
--- a/python/ur_simple_control/util/get_model.py
+++ b/python/ur_simple_control/util/get_model.py
@@ -234,3 +234,78 @@ def heron_approximation():
geom.meshScale = s
return model, visual_model.copy(), visual_model, data
+
+# TODO:
+# try building the mobile base as a rotational joint,
+# on top of which is a prismatic joint.
+# this way you get the differential-drive basis that you want,
+# and you don't have to manually correct the additional
+# degree of freedom everywhere (jacobian, control etc)
+def heron_approximationDD():
+ # arm + gripper
+ model_arm, collision_model_arm, visual_model_arm, data_arm = get_model()
+
+ # mobile base as planar joint (there's probably a better
+ # option but whatever right now)
+ model_mobile_base = pin.Model()
+ model_mobile_base.name = "mobile_base"
+ geom_model_mobile_base = pin.GeometryModel()
+ revolute_joint_name = "mobile_base_rotational_joint"
+ prismatic_joint_name = "mobile_base_prismatic_joint"
+ parent_id = 0
+ # TEST
+ joint_placement = pin.SE3.Identity()
+ #joint_placement.rotation = pin.rpy.rpyToMatrix(0, -np.pi/2, 0)
+ #joint_placement.translation[2] = 0.2
+ MOBILE_BASE_REVOLUTE_ID = model_mobile_base.addJoint(parent_id,
+ pin.JointModelRZ(),
+ joint_placement.copy(), revolute_joint_name)
+ MOBILE_BASE_PRISMATIC_ID = model_mobile_base.addJoint(
+ MOBILE_BASE_REVOLUTE_ID,
+ pin.JointModelPY(),
+ joint_placement.copy(), prismatic_joint_name)
+ # we should immediately set velocity limits.
+ # there are no position limit by default and that is what we want.
+ # TODO: put in heron's values
+ model_mobile_base.velocityLimit[0] = 2
+ model_mobile_base.velocityLimit[1] = 2
+# model_mobile_base.velocityLimit[2] = 2
+ # TODO: i have literally no idea what reasonable numbers are here
+ model_mobile_base.effortLimit[0] = 200
+ model_mobile_base.effortLimit[1] = 200
+# model_mobile_base.effortLimit[2] = 200
+ #print("OBJECT_JOINT_ID",OBJECT_JOINT_ID)
+ #body_inertia = pin.Inertia.FromBox(args.box_mass, box_dimensions[0],
+ # box_dimensions[1], box_dimensions[2])
+
+ # pretty much random numbers
+ # TODO: find heron (mir) numbers
+ body_inertia = pin.Inertia.FromBox(30, 0.5,
+ 0.3, 0.4)
+ # maybe change placement to sth else depending on where its grasped
+ model_mobile_base.appendBodyToJoint(MOBILE_BASE_PRISMATIC_ID,
+ body_inertia, pin.SE3.Identity())
+ box_shape = fcl.Box(0.5, 0.3, 0.4)
+ body_placement = pin.SE3.Identity()
+ geometry_mobile_base = pin.GeometryObject("box_shape", MOBILE_BASE_PRISMATIC_ID, box_shape, body_placement.copy())
+
+ geometry_mobile_base.meshColor = np.array([1., 0.1, 0.1, 1.])
+ geom_model_mobile_base.addGeometryObject(geometry_mobile_base)
+
+ # have to add the frame manually
+ model_mobile_base.addFrame(pin.Frame('base',MOBILE_BASE_PRISMATIC_ID,0,joint_placement.copy(),pin.FrameType.JOINT) )
+
+ # frame-index should be 1
+ model, visual_model = pin.appendModel(model_mobile_base, model_arm, geom_model_mobile_base, visual_model_arm, 1, pin.SE3.Identity())
+ data = model.createData()
+
+ # fix gripper
+ for geom in visual_model.geometryObjects:
+ if "hand" in geom.name:
+ s = geom.meshScale
+ geom.meshcolor = np.array([1.0, 0.1, 0.1, 1.0])
+ # this looks exactly correct lmao
+ s *= 0.001
+ geom.meshScale = s
+
+ return model, visual_model.copy(), visual_model, data