diff --git a/examples/regrasping/crocoddyl/pendulum/data/history_latest b/examples/regrasping/crocoddyl/pendulum/data/history_latest new file mode 100644 index 0000000000000000000000000000000000000000..d0c08f1819bbcf863cf91e991e1a1bded94cb36c Binary files /dev/null and b/examples/regrasping/crocoddyl/pendulum/data/history_latest differ diff --git a/examples/regrasping/crocoddyl/pendulum/fwd_sim_pendulum_scipy.py b/examples/regrasping/crocoddyl/pendulum/fwd_sim_pendulum_scipy.py index a76255a9edeb29a710572192ed5f74a0e8b0cf80..2c67b0db8d287521b7eae61f3d5560d6fb3d90ca 100644 --- a/examples/regrasping/crocoddyl/pendulum/fwd_sim_pendulum_scipy.py +++ b/examples/regrasping/crocoddyl/pendulum/fwd_sim_pendulum_scipy.py @@ -51,7 +51,10 @@ def stateEquation( TODO: pass other arguments as a tuple args=(...), TODO: fix evil pythonic memory accessing of variables outside of the function scope """ - x_dot = np.zeros(5) + if args.friction_model == "lugre": + x_dot = np.zeros(5) + else: + x_dot = np.zeros(4) q = x[:2] v = x[2:4] @@ -83,8 +86,10 @@ def stateEquation( tau[0] = u_tau_1 tau[1] = F_f q_ddot = pin.aba(model_pin, data_pin, q, v, tau) - x_dot[:4] = q_ddot - x_dot[4] = data_actuation.z_dot + x_dot[:2] = v + x_dot[2:4] = q_ddot + if args.friction_model == "lugre": + x_dot[4] = data_actuation.z_dot if history != None: history["q"][i] = q.copy() @@ -92,7 +97,7 @@ def stateEquation( history["tau"][i] = tau.copy() history["F_f"][i] = F_f.copy() - return x_dot + return x_dot def fwdSimPendulum( @@ -111,13 +116,17 @@ def fwdSimPendulum( t_init = 0 t_final = T * dt history = None + if args.friction_model == "lugre" and len(x0) < 5: + x00 = np.zeros(5) + x00[:4] = x0 + x0 = x00 result = solve_ivp( stateEquation, [t_init, t_final], x0, method="LSODA", - max_step=args.dt, + max_step=args.fwd_sim_dt, args=( args, 0, @@ -135,6 +144,7 @@ def fwdSimPendulum( "t": np.zeros((N,)), "q": np.zeros((N, 2)), "v": np.zeros((N, 2)), + "tau": np.zeros((N, 2)), "z": np.zeros((N,)), "F_f": np.zeros((N,)), "q_ddot": np.zeros((N, 2)), @@ -157,3 +167,72 @@ def fwdSimPendulum( t_from_ocp, history, ) + return history + + +if __name__ == "__main__": + from utils import get_args + import pickle + import matplotlib.pyplot as plt + import example_robot_data + from pendulum_croco_models import ActuationModelDoublePendulum + from pendulum_friction_computation import computeStickingCondition + + args = get_args() + dt = 1e-3 + T = 50 + x0 = np.random.random(4) + t = np.linspace(0, T * dt, T) + + # Loading the double pendulum model + pendulum = example_robot_data.load("double_pendulum") + pendulum.model.effortLimit[0] = 10 + pendulum.model.effortLimit[1] = 10 + model = pendulum.model + data = pendulum.data + + state = crocoddyl.StateMultibody(model) + if args.friction_model == "lugre": + state = MultiBodyWithLugreState(state) + # this is the first link + actuation = ActuationModelDoublePendulum(state, args, pendulum.model, pendulum.data) + + # here we make up some random us just to have something to simulate + phase = np.linspace(0, 10, T) + us = np.zeros((T, 2)) + us[:, 0] = np.sin(phase) + # us[:, 1] = np.cos(phase) + us[:, 1] = np.random.random(T) * 10 + + history = fwdSimPendulum( + args, + dt, + T, + x0, + t[-1], + actuation.mu_tors, + pendulum.model, + pendulum.data, + actuation.createData(), + us, + t, + ) + sticking_conditions = [] + xs = np.hstack((history["q"], history["v"])) + for x, u in zip(xs, us): + sticking_conditions.append( + computeStickingCondition( + actuation.mu_tors, pendulum.model, pendulum.data, x, u + ) + ) + plt.plot(np.arange(len(sticking_conditions)), sticking_conditions) + plt.title("sticking is 1, sliding is 0") + plt.show() + f = open("./data/history_latest", "wb") + pickle.dump(history, f) + f.close() + plt.plot(history["t"], history["q"], label="q") + plt.plot(history["t"], history["v"], label="v") + plt.legend() + plt.title("qs from fwd simulator") + plt.show() diff --git a/examples/regrasping/crocoddyl/pendulum/pendulum_regrasp.py b/examples/regrasping/crocoddyl/pendulum/pendulum_regrasp.py index 9dae3590ba0c25d19e290434cbbdf01ee73acc37..3405e796748427a10d33eb7ca1af245ed9b3c7c6 100644 --- a/examples/regrasping/crocoddyl/pendulum/pendulum_regrasp.py +++ b/examples/regrasping/crocoddyl/pendulum/pendulum_regrasp.py @@ -2,6 +2,7 @@ from pendulum_croco_models import ( ActuationModelDoublePendulum, CostModelDoublePendulum, ) +from fwd_sim_pendulum_scipy import fwdSimPendulum from utils import get_args from pendulum_friction_computation import computeStickingCondition from multibody_with_lugre_state import MultiBodyWithLugreState @@ -12,6 +13,7 @@ import signal import time import example_robot_data import numpy as np +import pickle args = get_args() @@ -101,7 +103,28 @@ xs = np.array(log.xs) us = np.array(log.us) t = np.linspace(0, T * dt, T) -xs_good_simulation = fwdSimPendulum() +# args.friction_model = "lugre" +history = fwdSimPendulum( + args, + dt, + T, + x0, + t[-1], + actuation.mu_tors, + pendulum.model, + pendulum.data, + actuation.createData(), + us, + t, +) +f = open("./data/history_latest", "wb") +pickle.dump(history, f) +f.close() +plt.plot(history["t"], history["q"], label="q") +plt.plot(history["t"], history["v"], label="v") +plt.legend() +plt.title("qs from fwd simulator") +plt.show() print( "both of these should be the same x0, but they're sometimes not, so please check things make sense :)" diff --git a/examples/regrasping/crocoddyl/utils.py b/examples/regrasping/crocoddyl/utils.py index 15da30f1fe77bc50e3ef9783f4df2026bdae7bff..86d6ed080df343972327ce2aaa0aa504033ad96e 100644 --- a/examples/regrasping/crocoddyl/utils.py +++ b/examples/regrasping/crocoddyl/utils.py @@ -15,12 +15,6 @@ def get_args(): parser.add_argument("--box-width", type=float, help="box width", default=0.07) parser.add_argument("--box-height", type=float, help="box height", default=0.02) parser.add_argument("--box-mass", type=float, help="box mass", default=0.800) - parser.add_argument( - "--control-gripping-force", - action=argparse.BooleanOptionalAction, - default=True, - help="whether you get to control the grasping force. if not, there is no torque on the object", - ) parser.add_argument( "--contact-point-x", type=float, @@ -33,9 +27,21 @@ def get_args(): help="contact point in y direction as percentage of width", default=0.3, ) + parser.add_argument( + "--fwd-sim-dt", + type=float, + default=1e-3, + help="after solving the ocp, we fwd sim the control inputs with a proper stiff equation solver - this is its max dt", + ) ############## # friction # ############## + parser.add_argument( + "--control-gripping-force", + action=argparse.BooleanOptionalAction, + default=True, + help="whether you get to control the grasping force. if not, there is no torque on the object", + ) parser.add_argument( "--friction-model", type=str,