From 83f8d69168c50efc481c397fdc1eefb19230043e Mon Sep 17 00:00:00 2001 From: dmronga Date: Wed, 6 Mar 2024 15:22:23 +0100 Subject: [PATCH] Add tutorial on TSID with Kuka robot --- tutorials/kuka_iiwa/CMakeLists.txt | 7 + tutorials/kuka_iiwa/cart_pos_ctrl_dynamic.cpp | 152 ++++++++++++++++++ 2 files changed, 159 insertions(+) create mode 100644 tutorials/kuka_iiwa/cart_pos_ctrl_dynamic.cpp diff --git a/tutorials/kuka_iiwa/CMakeLists.txt b/tutorials/kuka_iiwa/CMakeLists.txt index 50208ea3..a26fb3b2 100644 --- a/tutorials/kuka_iiwa/CMakeLists.txt +++ b/tutorials/kuka_iiwa/CMakeLists.txt @@ -25,3 +25,10 @@ target_link_libraries(cart_pos_ctrl_hls_hierarchies wbc-scenes-velocity wbc-robot_models-pinocchio wbc-controllers) + +add_executable(cart_pos_ctrl_dynamic cart_pos_ctrl_dynamic.cpp) +target_link_libraries(cart_pos_ctrl_dynamic + wbc-solvers-qpoases + wbc-scenes-acceleration_tsid + wbc-robot_models-pinocchio + wbc-controllers) diff --git a/tutorials/kuka_iiwa/cart_pos_ctrl_dynamic.cpp b/tutorials/kuka_iiwa/cart_pos_ctrl_dynamic.cpp new file mode 100644 index 00000000..933a4133 --- /dev/null +++ b/tutorials/kuka_iiwa/cart_pos_ctrl_dynamic.cpp @@ -0,0 +1,152 @@ +#include +#include +#include +#include +#include +#include + +using namespace std; +using namespace wbc; +using namespace wbc; + +/** + * Acceleration-based example, Cartesian position control of the KUKA iiwa robot (fixed base, no contacts). + * In the example the following problem is solved: + * \f[ + * \begin{array}{ccc} + * minimize & \| \mathbf{J}\ddot{\mathbf{q}} - \dot{\mathbf{v}}_d + \dot{\mathbf{J}}\dot{\mathbf{q}}\|_2\\ + * \mathbf{\ddot{q}},\mathbf{\tau} & & \\ + * s.t. & \mathbf{H}\mathbf{\ddot{q}} - \mathbf{\tau} - \mathbf{J}_{c,i}^T\mathbf{f} = -\mathbf{h}, \, \forall i & \\ + * & \mathbf{\tau}_m \leq \mathbf{\tau} \leq \mathbf{\tau}_M& \\ + * \end{array} + * \f] + * where + * \f[ + * \dot{\mathbf{v}}_d = \dot{\mathbf{v}}_r + \mathbf{K}_d(\mathbf{v}_r-\mathbf{v}) + \mathbf{K}_p(\mathbf{x}_r-\mathbf{x}) + * \f] + * \f$\ddot{\mathbf{q}}\f$ - Vector of robot joint accelerations
+ * \f$\dot{\mathbf{v}}_{d}\f$ - desired spatial acceleration / controller output
+ * \f$\mathbf{\tau}\f$ - actuation forces/torques
+ * \f$\mathbf{J}\f$ - task Jacobian
+ * \f$\mathbf{H}\f$ - Joint space inertia matrix
+ * \f$\mathbf{h}\f$ - bias forces/torques
+ * \f$\mathbf{\tau}_m,\mathbf{\tau}_M\f$ - Joint force/torque limits
+ * \f$\mathbf{x}_r, \mathbf{x}\f$ - Reference pose, actual pose
+ * \f$\mathbf{v}_r, \mathbf{v}\f$ - Reference spatial velocity, actual spatial velocity
+ * \f$\dot{\mathbf{v}}_r\f$ - Reference spatial acceleration (feed forward)
+ * \f$\mathbf{K}_p, \mathbf{K}_d\f$ - Proportional, derivative gain matrix
+ * + * The end effector is supposed to follow a sinusoidal trajectory. The solver computes the joint accelerations and forces/torques required + * to generate the desired spatial accelerations given by the Cartesian controller. + */ +int main() +{ + double dt = 0.01; + + // Create robot model, use hyrodyn-based model in this case + RobotModelPtr robot_model = make_shared(); + + // Configure robot model. We configure a serial model without parallel mechanisms. + // Blacklist the finger joints, as they are not relevant and may slow down the solver + RobotModelConfig config("../../../models/kuka/urdf/kuka_iiwa.urdf"); + if(!robot_model->configure(config)) + return -1; + + // Configure solver, use QPOases + QPSolverPtr solver = make_shared(); + dynamic_pointer_cast(solver)->setMaxNoWSR(1000); + qpOASES::Options options = dynamic_pointer_cast(solver)->getOptions(); + options.printLevel = qpOASES::PL_NONE; + dynamic_pointer_cast(solver)->setOptions(options); + + // Configure the AccelerationSceneTSID scene. This scene computes joint accelerations, joint torques and contact wrenches as output. + // Pass two tasks here: Left arm Cartesian pose and right arm Cartesian pose. + AccelerationSceneTSID scene(robot_model, solver, dt); + vector wbc_config; + wbc_config.push_back(TaskConfig("cart_pos_ctrl", 0, "kuka_lbr_l_link_0", "kuka_lbr_l_tcp", "kuka_lbr_l_link_0", 1.0)); + if(!scene.configure(wbc_config)) + return -1; + + // Choose an initial joint state. Since we use acceleration-based WBC here, we have to pass the velocities as well + base::samples::Joints joint_state; + uint nj = robot_model->noOfJoints(); + joint_state.resize(nj); + joint_state.names = robot_model->jointNames(); + for(uint i = 0; i < nj; i++) + joint_state[i].position = joint_state[i].speed = 0.1; + joint_state.time = base::Time::now(); + + // Configure Cartesian controller. The controller implements the following control law: + // + // a_d = kf*a_r + kd*(v_r - v) + kp(x_r - x). + // + // As we don't use feed forward acceleration here, we can ignore the factor kf. + CartesianPosPDController ctrl; + base::VectorXd p_gain(6),d_gain(6),ff_gain(6); + p_gain.setConstant(10); // Stiffness + d_gain.setConstant(30); // Damping + ff_gain.setConstant(1); // Feed forward + ctrl.setPGain(p_gain); + ctrl.setDGain(d_gain); + ctrl.setDGain(ff_gain); + + // Choose a valid reference pose x_r + base::samples::RigidBodyStateSE3 setpoint, ctrl_output, feedback; + setpoint.pose.position = base::Vector3d(0.0, 0.0, 1.0); + setpoint.pose.orientation.setIdentity(); + setpoint.twist.linear.setZero(); + setpoint.twist.angular.setZero(); + setpoint.frame_id = wbc_config[0].ref_frame; + + // Run control loop + JointIntegrator integrator; + double loop_time = dt; // seconds + base::commands::Joints solver_output; + for(double t = 0; t < 10; t+=loop_time){ + + // Update the robot model. WBC will only work if at least one joint state with valid timestamp has been passed to the robot model. + robot_model->update(joint_state); + + // Update controllers, left arm: Follow sinusoidal trajectory + // setpoint_left.pose.position[0] = 0.522827 + 0.1*sin(t); + // setpoint_left.twist.linear[0] = 0.1*cos(t); + // setpoint_left.acceleration.linear[0] = -0.1*sin(t); + feedback = robot_model->rigidBodyState(wbc_config[0].root, wbc_config[0].tip); + ctrl_output = ctrl.update(setpoint, feedback); + setpoint.pose.position[2] = 1.0 + 0.1*sin(t); + setpoint.twist.linear[2] = 0.1*cos(t); + + // Update constraints. Pass the control output of the controller to the corresponding constraint. + // The control output is the gradient of the task function that is to be minimized during execution. + scene.setReference(wbc_config[0].name, ctrl_output); + + // Update WBC scene. The output is a (hierarchical) quadratic program (QP), which can be solved by any standard QP solver + HierarchicalQP hqp = scene.update(); + + // Solve the QP. The output is the joint acceleration/torque that achieves the task space acceleration demanded by the controllers + solver_output = scene.solve(hqp); + + // Integrate once to get joint velocity from solver output + integrator.integrate(joint_state,solver_output,loop_time); + + // Update joint state by integration again + for(size_t i = 0; i < joint_state.size(); i++){ + joint_state[i].position = solver_output[i].position; + joint_state[i].speed = solver_output[i].speed; + } + printf("setpoint: x: %2.4f y: %2.4f z: %2.4f\n", setpoint.pose.position(0), setpoint.pose.position(1), setpoint.pose.position(2)); + printf("setpoint: qx: %2.4f qy: %2.4f qz: %2.4f qw: %2.4f\n\n", setpoint.pose.orientation.x(), setpoint.pose.orientation.y(), setpoint.pose.orientation.z(), setpoint.pose.orientation.w()); + printf("feedback: x: %2.4f y: %2.4f z: %2.4f\n", feedback.pose.position(0), feedback.pose.position(1), feedback.pose.position(2)); + printf("feedback: qx: %2.4f qy: %2.4f qz: %2.4f qw: %2.4f\n\n", feedback.pose.orientation.x(), feedback.pose.orientation.y(), feedback.pose.orientation.z(), feedback.pose.orientation.w()); + + printf("Solver output: "); printf("\n"); + printf("Joint Names: "); for(int i = 0; i < nj; i++) printf("%s ", solver_output.names[i].c_str()); printf("\n"); + printf("Velocity: "); for(int i = 0; i < nj; i++) printf("%2.4f ", solver_output[i].speed); printf("\n"); + printf("Acceleration: "); for(int i = 0; i < nj; i++) printf("%2.4f ", solver_output[i].acceleration); printf("\n"); + printf("Torque: "); for(int i = 0; i < nj; i++) printf("%2.4f ", solver_output[i].effort); printf("\n"); + printf("---------------------------------------------------------------------------------------------\n\n"); + usleep(loop_time * 1e6); + } + + return 0; +}