Loading...
Searching...
No Matches
Robot manipulator example of robotoc::UnconstrOCPSolver and robotoc::UnconstrParNMPCSolver in Python
This page explains the example code in examples/iiwa14/python/config_space_ocp.py.
Required imports are as follows.
First, define the robot model.
model_info = robotoc.RobotModelInfo()
model_info.urdf_path = "../iiwa_description/urdf/iiwa14.urdf"
robot.set_joint_effort_limit(np.full(robot.dimu(), 50)) # change the joint limits
robot.set_joint_velocity_limit(np.full(robot.dimv(), 0.5*math.pi)) # change the joint limits
Dynamics and kinematics model of robots. Wraps pinocchio::Model and pinocchio::Data....
Definition: robot.hpp:32
std::string urdf_path
Path to the URDF file.
Definition: robot_model_info.hpp:73
Next, we construct the cost function.
cost = robotoc.CostFunction()
config_cost = robotoc.ConfigurationSpaceCost(robot)
q_ref = np.array([0, 0.5*math.pi, 0, 0.5*math.pi, 0, 0.5*math.pi, 0])
config_cost.set_q_ref(q_ref)
config_cost.set_q_weight(np.full(robot.dimv(), 10))
config_cost.set_q_weight_terminal(np.full(robot.dimv(), 10))
config_cost.set_v_weight(np.full(robot.dimv(), 0.01))
config_cost.set_v_weight_terminal(np.full(robot.dimv(), 0.01))
config_cost.set_a_weight(np.full(robot.dimv(), 0.01))
cost.add("config_cost", config_cost)
void set_q_ref(const Eigen::VectorXd &q_ref)
Sets the const reference configuration q.
Stack of the cost function. Composed by cost function components that inherits CostFunctionComponentB...
Definition: cost_function.hpp:30
Next, we construct the constraints.
constraints = robotoc.Constraints(barrier_param=1.0e-03, fraction_to_boundary_rule=0.995)
joint_position_lower = robotoc.JointPositionLowerLimit(robot)
joint_position_upper = robotoc.JointPositionUpperLimit(robot)
joint_velocity_lower = robotoc.JointVelocityLowerLimit(robot)
joint_velocity_upper = robotoc.JointVelocityUpperLimit(robot)
joint_torques_lower = robotoc.JointTorquesLowerLimit(robot)
joint_torques_upper = robotoc.JointTorquesUpperLimit(robot)
constraints.add("joint_position_lower", joint_position_lower)
constraints.add("joint_position_upper", joint_position_upper)
constraints.add("joint_velocity_lower", joint_velocity_lower)
constraints.add("joint_velocity_upper", joint_velocity_upper)
constraints.add("joint_torques_lower", joint_torques_lower)
constraints.add("joint_torques_upper", joint_torques_upper)
Stack of the inequality constraints. Composed by constraint components that inherits ConstraintCompon...
Definition: constraints.hpp:30
Constraint on the lower limits of the joint position.
Definition: joint_position_lower_limit.hpp:22
Constraint on the upper limits of the joint position.
Definition: joint_position_upper_limit.hpp:22
Constraint on the lower limits of the joint torques.
Definition: joint_torques_lower_limit.hpp:22
Constraint on the upper limits of the joint torques.
Definition: joint_torques_upper_limit.hpp:22
Constraint on the lower limits of the joint velocity.
Definition: joint_velocity_lower_limit.hpp:22
Constraint on the upper limits of the joint velocity.
Definition: joint_velocity_upper_limit.hpp:22
Finally, we can construct the optimal control solver!
T = 3.0
N = 60
solver_options = robotoc.SolverOptions()
solver_options.nthreads = 4
ocp_solver = robotoc.UnconstrOCPSolver(ocp=ocp, solver_options=solver_options)
Optimal control problem solver of unconstrained rigid-body systems by Riccati recursion....
Definition: unconstr_ocp_solver.hpp:35
int nthreads
Number of the threads of parallel computations. Must be positive. Default is 1.
Definition: solver_options.hpp:22
or ParNMPC solver!
T = 3.0
N = 60
solver_options = robotoc.SolverOptions()
solver_options.nthreads = 4
ocp_solver = robotoc.UnconstrParNMPCSolver(ocp=ocp, solver_options=solver_options)
Optimal control problem solver of unconstrained rigid-body systems by ParNMPC algorithm....
Definition: unconstr_parnmpc_solver.hpp:34
Let's run the solver!
t = 0.
q = np.array([0.5*math.pi, 0, 0.5*math.pi, 0, 0.5*math.pi, 0, 0.5*math.pi]) # initial state.
v = np.zeros(robot.dimv()) # initial state.
ocp_solver.set_solution("q", q) # set the initial guess of the solution.
ocp_solver.set_solution("v", v) # set the initial guess of the solution.
ocp_solver.init_constraints() # initialize the slack and dual variables of the primal-dual interior point method.
print("Initial KKT error: ", ocp_solver.KKT_error(t, q, v))
ocp_solver.solve(t, q, v, init_solver=True)
print("KKT error after convergence: ", ocp_solver.KKT_error(t, q, v))
print(ocp_solver.get_solver_statistics()) # print solver statistics
We can visualize the solution trajectory as
viewer = robotoc.utils.TrajectoryViewer(model_info=model_info, viewer_type='meshcat')
viewer.set_camera_transform_meshcat(camera_tf_vec=[0.5, -3.0, 0.0], zoom=2.0)
viewer.display(ocp_solver.get_time_discretization(),
ocp_solver.get_solution('q'))
