diff --git a/python/examples/crocoddyl_ocp_clik.py b/python/examples/crocoddyl_ocp_clik.py
index 84d2a1cea22016e8810f8796c449ace62f6f2b8e..4763547af329a0c642f3441909079a728d079934 100644
--- a/python/examples/crocoddyl_ocp_clik.py
+++ b/python/examples/crocoddyl_ocp_clik.py
@@ -4,9 +4,11 @@ import argparse
from functools import partial
from ur_simple_control.managers import getMinimalArgParser, ControlLoopManager, RobotManager
from ur_simple_control.optimal_control.crocoddyl_optimal_control import get_OCP_args, CrocoIKOCP
-from ur_simple_control.basics.basics import jointTrajFollowingPD
+from ur_simple_control.basics.basics import followKinematicJointTrajP
from ur_simple_control.util.logging_utils import LogManager
+from ur_simple_control.visualize.visualize import plotFromDict
import pinocchio as pin
+import crocoddyl
def get_args():
@@ -19,22 +21,26 @@ def get_args():
if __name__ == "__main__":
args = get_args()
robot = RobotManager(args)
- Mgoal = robot.defineGoalPointCLI()
+ # TODO: put this back for nicer demos
+ #Mgoal = robot.defineGoalPointCLI()
+ Mgoal = pin.SE3.Random()
# create and solve the optimal control problem of
# getting from current to goal end-effector position.
# reference is position and velocity reference (as a dictionary),
# while solver is a crocoddyl object containing a lot more information
reference, solver = CrocoIKOCP(args, robot, Mgoal)
+ log = solver.getCallbacks()[1]
+ crocoddyl.plotOCSolution(log.xs, log.us, figIndex=1, show=True)
# we need a way to follow the reference trajectory,
# both because there can be disturbances,
# and because it is sampled at a much lower frequency
- jointTrajFollowingPD(args, robot, reference)
- #reference.pop('dt')
- #plotFromDict(reference, args.n_knots + 1, args)
+ followKinematicJointTrajP(args, robot, reference)
print("final position:")
print(robot.getT_w_e())
+ robot.log_manager.plotAllControlLoops()
+
if not args.pinocchio_only:
robot.stopRobot()
diff --git a/python/examples/data/latest_run b/python/examples/data/latest_run
new file mode 100644
index 0000000000000000000000000000000000000000..107c2246481edf0fd0cf7df9ee13b5c4eed62b68
Binary files /dev/null and b/python/examples/data/latest_run differ
diff --git a/python/examples/heron_pls.py b/python/examples/heron_pls.py
index 346ff92fe46f84f3190c2710a0860472e3920ad9..5eb9f9153e58c6c05afc8bd116abcd3ef36619df 100644
--- a/python/examples/heron_pls.py
+++ b/python/examples/heron_pls.py
@@ -11,14 +11,13 @@ import pinocchio as pin
def get_args():
parser = getMinimalArgParser()
parser.description = 'mpc for heron path following'
- parser = get_OCP_args(parser)
args = parser.parse_args()
return args
if __name__ == "__main__":
args = get_args()
- model, collision_model, visual_model, data = heron_approximation(args)
+ model, collision_model, visual_model, data = heron_approximation()
print(model)
viz = MeshcatVisualizer(model, collision_model, visual_model)
viz.initViewer(open=True)
diff --git a/python/ur_simple_control/__pycache__/managers.cpython-312.pyc b/python/ur_simple_control/__pycache__/managers.cpython-312.pyc
index eebd716670a2552ef6b2850384768b390ae8cf3d..88c6cb821ec8dc97c3b71076271c9edadd6dadf8 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/basics/__pycache__/basics.cpython-312.pyc b/python/ur_simple_control/basics/__pycache__/basics.cpython-312.pyc
index 1faaaed581604ec4087a48d6b79a04fa2e648a92..0e87b163ebbfe0936a10a8fdbb724fb20f9fdd74 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 c8f48d08b458601ee23f36b92d2cfcec641dd81a..ed1d53be8dc25f800093d5ad2d4c3e92c12e98c2 100644
--- a/python/ur_simple_control/basics/basics.py
+++ b/python/ur_simple_control/basics/basics.py
@@ -72,7 +72,7 @@ def moveJP(args, robot, q_desired):
# also NOTE: there is an argument to be made for pre-interpolating the reference.
# this is because joint positions will be more accurate.
# if we integrate them with interpolated velocities.
-def jointTrajFollowingPDControlLoop(stop_at_final : bool, robot: RobotManager, reference, i, past_data):
+def followKinematicJointTrajPControlLoop(stop_at_final : bool, robot: RobotManager, reference, i, past_data):
breakFlag = False
save_past_dict = {}
log_item = {}
@@ -82,11 +82,10 @@ def jointTrajFollowingPDControlLoop(stop_at_final : bool, robot: RobotManager, r
# which is pretty much true
t = i * robot.dt
# take the future (next) one
- # TODO: turn this into interpolation
t_index_lower = int(np.floor(t / reference['dt']))
t_index_upper = int(np.ceil(t / reference['dt']))
- # TODO: move to a different function later
+ # TODO: set q_refs and v_refs once (merge with interpolation if)
if t_index_upper >= len(reference['qs']) - 1:
t_index_upper = len(reference['qs']) - 1
q_ref = reference['qs'][t_index_upper]
@@ -95,17 +94,13 @@ def jointTrajFollowingPDControlLoop(stop_at_final : bool, robot: RobotManager, r
else:
v_ref = reference['vs'][t_index_upper]
- # TODO: check if that's really the last
- #if t_index_upper == len(reference['qs']) - 1:
- # breakFlag = True
-
# TODO NOTE TODO TODO: move under stop/don't stop at final argument
if (t_index_upper == len(reference['qs']) - 1) and \
(np.linalg.norm(q - reference['qs'][-1]) < 1e-2) and \
(np.linalg.norm(v) < 1e-2):
breakFlag = True
- # TODO: set q_refs and v_refs once
+ # TODO: move interpolation to a different function later
if (t_index_upper < len(reference['qs']) - 1) and (not breakFlag):
#angle = (reference['qs'][t_index_upper] - reference['qs'][t_index_lower]) / reference['dt']
angle_v = (reference['vs'][t_index_upper] - reference['vs'][t_index_lower]) / reference['dt']
@@ -118,8 +113,6 @@ def jointTrajFollowingPDControlLoop(stop_at_final : bool, robot: RobotManager, r
q_ref = pin.integrate(robot.model, reference['qs'][t_index_lower], reference['vs'][t_index_lower] * time_difference)
-
-
if robot.robot_name == "ur5e":
error_q = q_ref - q
if robot.robot_name == "heron":
@@ -143,9 +136,9 @@ def jointTrajFollowingPDControlLoop(stop_at_final : bool, robot: RobotManager, r
return breakFlag, {}, log_item
-def jointTrajFollowingPD(args, robot, reference):
+def followKinematicJointTrajP(args, robot, reference):
# we're not using any past data or logging, hence the empty arguments
- controlLoop = partial(jointTrajFollowingPDControlLoop, args.stop_at_final, robot, reference)
+ controlLoop = partial(followKinematicJointTrajPControlLoop, args.stop_at_final, robot, reference)
log_item = {
'error_qs' : np.zeros(robot.model.nq),
'error_vs' : np.zeros(robot.model.nv),
diff --git a/python/ur_simple_control/managers.py b/python/ur_simple_control/managers.py
index a23fba31207a3e68da78bce780f81d1f34983734..f96d0e303ebf81b3593e469ad4ffc057cbcb84b9 100644
--- a/python/ur_simple_control/managers.py
+++ b/python/ur_simple_control/managers.py
@@ -79,6 +79,8 @@ def getMinimalArgParser():
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('--fast-simulation', action=argparse.BooleanOptionalAction, \
+ help="do you want simulation to run over 500Hz? (real-time viz relies on 500Hz)", 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, \
@@ -116,7 +118,7 @@ def getMinimalArgParser():
help="name the whole run/experiment (name of log file). note that indexing of runs is automatic and under a different argument.", \
default='latest_run')
parser.add_argument('--index-runs', action=argparse.BooleanOptionalAction, \
- help="if you want more runs of the same name, this option will automatically assign an index to a new run (useful for data collection).", default=True)
+ help="if you want more runs of the same name, this option will automatically assign an index to a new run (useful for data collection).", default=False)
parser.add_argument('--past-window-size', type=int, \
help="how many timesteps of past data you want to save", default=5)
# maybe you want to scale the control signal (TODO: NOT HERE)
@@ -294,7 +296,12 @@ class ControlLoopManager:
print("missed deadline by", diff - self.robot_manager.dt)
continue
else:
- time.sleep(self.robot_manager.dt - diff)
+ # there's no point in sleeping if we're in simulation
+ # NOTE: it literally took me a year to put this if here
+ # and i have no idea why i didn't think of it before lmao
+ # (because i did know about it, just didn't even think of changing it)
+ if (not self.args.pinocchio_only) and (not self.args.fast_simulation):
+ time.sleep(self.robot_manager.dt - diff)
if self.args.debug_prints:
@@ -494,6 +501,10 @@ class RobotManager:
# also TODO: figure out how to best solve the gripper_velocity problem
# NOTE: you need to initialize differently for other types of joints
self.q = np.zeros(self.model.nq)
+ # planar joint is [x, y, cos(theta), sin(theta)],
+ # so it can't be all zeros
+ if self.robot_name == "heron":
+ self.q[2] = 1.0
# v_q is the generalization of qd for every type of joint.
# for revolute joints it's qd, but for ex. the planar joint it's the body velocity.
self.v_q = np.zeros(self.model.nv)
diff --git a/python/ur_simple_control/optimal_control/crocoddyl_optimal_control.py b/python/ur_simple_control/optimal_control/crocoddyl_optimal_control.py
index 8faf38ad94f7a9e28f5c3a89ff729de346cd8c1d..1869c54184fc2e100a911079cd43ab5dd5dba30c 100644
--- a/python/ur_simple_control/optimal_control/crocoddyl_optimal_control.py
+++ b/python/ur_simple_control/optimal_control/crocoddyl_optimal_control.py
@@ -3,7 +3,7 @@ import pinocchio as pin
import crocoddyl
from ur_simple_control.managers import getMinimalArgParser, ControlLoopManager, RobotManager
import argparse
-from ur_simple_control.basics.basics import jointTrajFollowingPD
+from ur_simple_control.basics.basics import followKinematicJointTrajP
from ur_simple_control.visualize.visualize import plotFromDict
import example_robot_data
import time
@@ -27,7 +27,7 @@ def CrocoIKOCP(args, robot : RobotManager, goal : pin.SE3):
#print(robot.model.effortLimit)
#exit()
#robot.model.effortLimit = robot.model.effortLimit * (args.acceleration / robot.MAX_ACCELERATION)
- #robot.model.effortLimit = robot.model.effortLimit * 0.01
+ #robot.model.effortLimit = robot.model.effortLimit * 0.5
#robot.data = robot.model.createData()
# starting state
x0 = np.concatenate([robot.getQ(), robot.getQd()])
@@ -82,32 +82,33 @@ def CrocoIKOCP(args, robot : RobotManager, goal : pin.SE3):
xub = np.concatenate([
robot.model.upperPositionLimit,
robot.model.velocityLimit])
+ # we have no limits on the mobile base.
+ # the mobile base is a planar joint.
+ # since it is represented as [x,y,cos(theta),sin(theta)], there is no point
+ # to limiting cos(theta),sin(theta) even if we wanted limits,
+ # because we would then limit theta, or limit ct and st jointly.
+ # in any event, xlb and xub are 1 number too long --
+ # the residual has to be [x,y,theta] because it is in the tangent space -
+ # the difference between two points on a manifold in pinocchio is defined
+ # as the velocity which if parallel transported for 1 unit of "time"
+ # from one to point to the other.
+ # point activation input and the residual need to be of the same length obviously,
+ # and this should be 2 * model.nv the way things are defined here.
+ if robot.robot_name == "heron":
+ xlb = xlb[1:]
+ xub = xub[1:]
bounds = crocoddyl.ActivationBounds(xlb, xub, 1.0)
- xLimitResidual = crocoddyl.ResidualModelState(state, x0, actuation.nu)
+ xLimitResidual = crocoddyl.ResidualModelState(state, x0, state.nv)
xLimitActivation = crocoddyl.ActivationModelQuadraticBarrier(bounds)
- # TOOODOOO ADD THE SAME FOR WHOLE HERON
- if robot.robot_name == "ur5e":
- limitCost = crocoddyl.CostModelResidual(state, xLimitActivation, xLimitResidual)
- runningCostModel.addCost("limitCost", limitCost, 1e3)
- terminalCostModel.addCost("limitCost", limitCost, 1e3)
+ print(len(x0), len(xlb), len(xub))
+ print(xLimitResidual.nr)
+ print(xLimitActivation.nr)
+ limitCost = crocoddyl.CostModelResidual(state, xLimitActivation, xLimitResidual)
+ runningCostModel.addCost("limitCost", limitCost, 1e3)
+ terminalCostModel.addCost("limitCost", limitCost, 1e3)
- # NOTE: i have no idea how to incorporate this into anything
- # we need to add state constraints for things to make sense
- # NOTE!!!: there are no hard inequality constraints on states
- # in what's implemented in crocoddyl.
- # INSTEAD, you define quadratic barier-type costs for that.
- # there are box constraints on control input though,
- # but i'm pretty sure i'm not using those correctly.
- #x_constraint_lower = x0.copy()
- #x_constraint_upper = x0.copy()
- #x_constraint_lower[:robot.model.nq] = -2 * np.pi
- #x_constraint_upper[:robot.model.nq] = 2 * np.pi
- #x_constraint_lower[robot.model.nq:] = -1 * robot.max_qd
- #x_constraint_upper[robot.model.nq:] = robot.max_qd
- #state_constraint = crocoddyl.ConstraintModelResidual(state, xResidual, x_constraint_lower, x_constraint_upper)
-
# Next, we need to create an action model for running and terminal knots. The
# forward dynamics (computed using ABA) are implemented
# inside DifferentialActionModelFreeFwdDynamics.
@@ -129,7 +130,7 @@ def CrocoIKOCP(args, robot : RobotManager, goal : pin.SE3):
# and the solver
solver = crocoddyl.SolverBoxFDDP(problem)
#solver = crocoddyl.SolverIpopt(problem)
- # TODO: remove / place elsewhere once it works
+ # TODO: remove / place elsewhere once it works (make it an argument)
solver.setCallbacks([crocoddyl.CallbackVerbose(), crocoddyl.CallbackLogger()])
# run solve
# NOTE: there are some possible parameters here that I'm not using
@@ -172,7 +173,7 @@ if __name__ == "__main__":
log = solver.getCallbacks()[1]
crocoddyl.plotOCSolution(log.xs, log.us, figIndex=1, show=True)
crocoddyl.plotConvergence(log.costs, log.pregs, log.dregs, log.grads, log.stops, log.steps, figIndex=2)
- jointTrajFollowingPD(args, robot, reference)
+ followKinematicJointTrajP(args, robot, reference)
reference.pop('dt')
plotFromDict(reference, args.n_knots + 1, args)
print("achieved result", robot.getT_w_e())
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 7fb311856ef9a0ccfa2de4ba05ba86e1b0356785..172fcd77d0c1c699029769b4d378712d1d374d19 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/__pycache__/logging_utils.cpython-312.pyc b/python/ur_simple_control/util/__pycache__/logging_utils.cpython-312.pyc
index fe80f5e9d71c11753cadb1118869190383545e4c..f87a89b70ea42b66252ea0a118e7eb21c5ad2837 100644
Binary files a/python/ur_simple_control/util/__pycache__/logging_utils.cpython-312.pyc and b/python/ur_simple_control/util/__pycache__/logging_utils.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 a5737ee1084517f9f6c1a1550e1d6d8680df75c6..ce274d50aaf896af13d3306a6f6c636e338ede11 100644
--- a/python/ur_simple_control/util/get_model.py
+++ b/python/ur_simple_control/util/get_model.py
@@ -125,8 +125,26 @@ def heron_approximation():
joint_placement = pin.SE3.Identity()
#joint_placement.rotation = pin.rpy.rpyToMatrix(0, -np.pi/2, 0)
#joint_placement.translation[2] = 0.2
+ # TODO TODO TODO TODO TODO TODO TODO TODO
+ # TODO: heron is actually a differential drive,
+ # meaning that it is not a planar joint.
+ # we could put in a prismatic + revolute joint
+ # as the base (both joints being at same position),
+ # and that should work for our purposes.
+ # this makes sense for initial testing
+ # because mobile yumi's base is a planar joint
MOBILE_BASE_JOINT_ID = model_mobile_base.addJoint(parent_id, pin.JointModelPlanar(),
joint_placement.copy(), 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])
diff --git a/python/ur_simple_control/util/logging_utils.py b/python/ur_simple_control/util/logging_utils.py
index 291500fb6ddb8b265d68cd0bad2abf2c75c4a265..948637e31866e25fdedb6a586cd9be16ecf795c7 100644
--- a/python/ur_simple_control/util/logging_utils.py
+++ b/python/ur_simple_control/util/logging_utils.py
@@ -3,6 +3,8 @@ import numpy as np
import os
import subprocess
import re
+from ur_simple_control.visualize.visualize import plotFromDict
+
class LogManager:
@@ -21,6 +23,7 @@ class LogManager:
if args is None:
return
self.args = args
+ self.are_logs_vectorized_flag = False
self.loop_logs = {}
self.loop_number = 0
# name the run
@@ -52,6 +55,11 @@ class LogManager:
self.loop_number += 1
self.loop_logs[loop_name] = log_dict
+ def vectorizeLog(self):
+ for control_loop_name in self.loop_logs:
+ for key in self.loop_logs[control_loop_name]:
+ self.loop_logs[control_loop_name][key] = np.array(self.loop_logs[control_loop_name][key])
+ self.are_logs_vectorized_flag = True
def saveLog(self, cleanUpRun=False):
"""
@@ -63,9 +71,8 @@ class LogManager:
Uses default pickling.
"""
# finally transfer to numpy (after nothing is running anymore)
- for control_loop_name in self.loop_logs:
- for key in self.loop_logs[control_loop_name]:
- self.loop_logs[control_loop_name][key] = np.array(self.loop_logs[control_loop_name][key])
+ if not self.are_logs_vectorized_flag:
+ self.vectorizeLog()
if cleanUpRun:
self.cleanUpRun()
print(f"data is ready, logmanager will now save your logs to \
@@ -88,6 +95,14 @@ class LogManager:
self.__dict__.clear()
self.__dict__.update(tmp_dict)
+ def plotAllControlLoops(self):
+ if not self.are_logs_vectorized_flag:
+ self.vectorizeLog()
+ self.are_logs_vectorized_flag = True
+
+ for control_loop_name in self.loop_logs:
+ plotFromDict(self.loop_logs[control_loop_name], len(self.loop_logs[control_loop_name]['qs']), self.args)
+
def findLatestIndex(self):
"""
diff --git a/python/ur_simple_control/visualize/__pycache__/visualize.cpython-312.pyc b/python/ur_simple_control/visualize/__pycache__/visualize.cpython-312.pyc
index 7039720b6b1d9858d08e7c3911c57c5324a85ff2..9dd45e1af3f798ed65e27a6b9dcc08e32431e013 100644
Binary files a/python/ur_simple_control/visualize/__pycache__/visualize.cpython-312.pyc and b/python/ur_simple_control/visualize/__pycache__/visualize.cpython-312.pyc differ
diff --git a/python/ur_simple_control/visualize/visualize.py b/python/ur_simple_control/visualize/visualize.py
index 0215e599f385bc8268641ecaa48baa462c7760d8..30221a7d14c635ff3f23ec225013d03145e9793b 100644
--- a/python/ur_simple_control/visualize/visualize.py
+++ b/python/ur_simple_control/visualize/visualize.py
@@ -34,14 +34,14 @@ def getNRowsMColumnsFromTotalNumber(n_plots):
return n_rows, n_cols
-"""
-plotFromDict
-------------
-plots logs stored in a dictionary
-- every key is one subplot, and it's value
- is what you plot
-"""
def plotFromDict(plot_data, final_iteration, args):
+ """
+ plotFromDict
+ ------------
+ plots logs stored in a dictionary
+ - every key is one subplot, and it's value
+ is what you plot
+ """
# TODO: replace with actual time ( you know what dt is from args)
t = np.arange(final_iteration)
n_cols, n_rows = getNRowsMColumnsFromTotalNumber(len(plot_data))