diff --git a/python/examples/data/latest_run b/python/examples/data/latest_run index 107c2246481edf0fd0cf7df9ee13b5c4eed62b68..87c7943d861356b43e2f96d2c2bf7f90abd4efac 100644 Binary files a/python/examples/data/latest_run and b/python/examples/data/latest_run differ 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 1869c54184fc2e100a911079cd43ab5dd5dba30c..e1db433a90b218ca1f63d51fa8fce3992a910c28 100644 --- a/python/ur_simple_control/optimal_control/crocoddyl_optimal_control.py +++ b/python/ur_simple_control/optimal_control/crocoddyl_optimal_control.py @@ -33,6 +33,7 @@ def CrocoIKOCP(args, robot : RobotManager, goal : pin.SE3): x0 = np.concatenate([robot.getQ(), robot.getQd()]) state = crocoddyl.StateMultibody(robot.model) # command input IS torque + # TODO: consider ActuationModelFloatingBaseTpl for heron actuation = crocoddyl.ActuationModelFull(state) # we will be summing 4 different costs @@ -94,6 +95,12 @@ def CrocoIKOCP(args, robot : RobotManager, goal : pin.SE3): # 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. + + # TODO: try using crocoddyl.ConstraintModelResidual + # instead to create actual box constraints (inequality constraints) + # TODO: same goes for control input + # NOTE: i'm not sure whether any solver uses these tho lel + if robot.robot_name == "heron": xlb = xlb[1:] xub = xub[1:] @@ -118,12 +125,16 @@ def CrocoIKOCP(args, robot : RobotManager, goal : pin.SE3): ), args.ocp_dt, ) + runningModel.u_lb = -1 * robot.model.effortLimit * 0.1 + runningModel.u_ub = robot.model.effortLimit * 0.1 terminalModel = crocoddyl.IntegratedActionModelEuler( crocoddyl.DifferentialActionModelFreeInvDynamics( state, actuation, terminalCostModel ), 0.0, ) + terminalModel.u_lb = -1 * robot.model.effortLimit * 0.1 + terminalModel.u_ub = robot.model.effortLimit * 0.1 # now we define the problem problem = crocoddyl.ShootingProblem(x0, [runningModel] * args.n_knots, terminalModel)