diff --git a/include/qrw/InvKin.hpp b/include/qrw/InvKin.hpp
index d584d939e8b61dbbc27026699ec223e443a2ef8c..4915c47a59becfbb53a8fde201e26d478d88a15e 100644
--- a/include/qrw/InvKin.hpp
+++ b/include/qrw/InvKin.hpp
@@ -48,11 +48,11 @@ private:
Eigen::Matrix<double, 6, 1> x = Eigen::Matrix<double, 6, 1>::Zero();
Eigen::Matrix<double, 6, 1> dx_ref = Eigen::Matrix<double, 6, 1>::Zero();
Eigen::Matrix<double, 6, 1> dx = Eigen::Matrix<double, 6, 1>::Zero();
- Eigen::Matrix<double, 18, 18> J = Eigen::Matrix<double, 18, 18>::Zero();
- Eigen::Matrix<double, 18, 18> invJ = Eigen::Matrix<double, 18, 18>::Zero();
- Eigen::Matrix<double, 1, 18> acc = Eigen::Matrix<double, 1, 18>::Zero();
- Eigen::Matrix<double, 1, 18> x_err = Eigen::Matrix<double, 1, 18>::Zero();
- Eigen::Matrix<double, 1, 18> dx_r = Eigen::Matrix<double, 1, 18>::Zero();
+ Eigen::Matrix<double, 12, 12> J = Eigen::Matrix<double, 12, 12>::Zero();
+ Eigen::Matrix<double, 12, 12> invJ = Eigen::Matrix<double, 12, 12>::Zero();
+ Eigen::Matrix<double, 1, 12> acc = Eigen::Matrix<double, 1, 12>::Zero();
+ Eigen::Matrix<double, 1, 12> x_err = Eigen::Matrix<double, 1, 12>::Zero();
+ Eigen::Matrix<double, 1, 12> dx_r = Eigen::Matrix<double, 1, 12>::Zero();
Eigen::Matrix<double, 4, 3> pfeet_err = Eigen::Matrix<double, 4, 3>::Zero();
Eigen::Matrix<double, 4, 3> vfeet_ref = Eigen::Matrix<double, 4, 3>::Zero();
@@ -62,9 +62,9 @@ private:
Eigen::Matrix<double, 1, 3> e_basisrot = Eigen::Matrix<double, 1, 3>::Zero();
Eigen::Matrix<double, 1, 3> awbasis = Eigen::Matrix<double, 1, 3>::Zero();
- Eigen::MatrixXd ddq = Eigen::MatrixXd::Zero(18, 1);
- Eigen::MatrixXd q_step = Eigen::MatrixXd::Zero(18, 1);
- Eigen::MatrixXd dq_cmd = Eigen::MatrixXd::Zero(18, 1);
+ Eigen::MatrixXd ddq = Eigen::MatrixXd::Zero(12, 1);
+ Eigen::MatrixXd q_step = Eigen::MatrixXd::Zero(12, 1);
+ Eigen::MatrixXd dq_cmd = Eigen::MatrixXd::Zero(12, 1);
// Gains
double Kp_base_orientation = 100.0;
@@ -73,8 +73,8 @@ private:
double Kp_base_position = 100.0;
double Kd_base_position = 2.0 * std::sqrt(Kp_base_position);
- double Kp_flyingfeet = 1000.0;
- double Kd_flyingfeet = 5.0 * std::sqrt(Kp_flyingfeet);
+ double Kp_flyingfeet = 100.0; // 1000
+ double Kd_flyingfeet = 2.0 * std::sqrt(Kp_flyingfeet); // 5.0 *
};
template <typename _Matrix_Type_>
diff --git a/scripts/Controller.py b/scripts/Controller.py
index cf5766158266e9b311b6d718d1ea6266059514f4..b249294c66f1bbb5de332ccf4ffa4e96f6b829cb 100644
--- a/scripts/Controller.py
+++ b/scripts/Controller.py
@@ -10,7 +10,7 @@ import pybullet as pyb
import pinocchio as pin
from solopython.utils.viewerClient import viewerClient, NonBlockingViewerFromRobot
import libquadruped_reactive_walking as lqrw
-
+from example_robot_data.robots_loader import Solo12Loader
class Result:
"""Object to store the result of the control loop
@@ -149,6 +149,7 @@ class Controller:
# Define the default controller
self.myController = wbc_controller(dt_wbc, N_SIMULATION)
+ self.myController.qdes[7:] = q_init.ravel()
self.envID = envID
self.velID = velID
@@ -169,10 +170,26 @@ class Controller:
self.qmes12 = np.zeros((19, 1))
self.vmes12 = np.zeros((18, 1))
- self.v_estim = np.zeros((18, 1))
+ self.v_ref = np.zeros((18, 1))
+ self.h_v = np.zeros((18, 1))
self.yaw_estim = 0.0
self.RPY_filt = np.zeros(3)
+ self.feet_a_cmd = np.zeros((3, 4))
+ self.feet_v_cmd = np.zeros((3, 4))
+ self.feet_p_cmd = np.zeros((3, 4))
+
+ # Solo12 model without free flyer
+ Solo12Loader.free_flyer = False
+ self.solo12_no_ff = Solo12Loader().robot
+ FL_FOOT_ID = self.solo12_no_ff.model.getFrameId('FL_FOOT')
+ FR_FOOT_ID = self.solo12_no_ff.model.getFrameId('FR_FOOT')
+ HL_FOOT_ID = self.solo12_no_ff.model.getFrameId('HL_FOOT')
+ HR_FOOT_ID = self.solo12_no_ff.model.getFrameId('HR_FOOT')
+ self.foot_ids_no_ff = np.array([FL_FOOT_ID, FR_FOOT_ID, HL_FOOT_ID, HR_FOOT_ID])
+ self.b_pos_feet = np.zeros((3, 4))
+ self.b_vel_feet = np.zeros((3, 4))
+
self.error_flag = 0
self.q_security = np.array([np.pi*0.4, np.pi*80/180, np.pi] * 4)
@@ -209,13 +226,16 @@ class Controller:
t_filter = time.time()
# Update state for the next iteration of the whole loop
- self.v_estim[0:3, 0] = self.joystick.v_ref[0:3, 0] # TODO: Joystick velocity given in base frame and not
- self.v_estim[3:6, 0] = self.joystick.v_ref[3:6, 0] # in horizontal frame (case of non flat ground)
- self.v_estim[6:, 0] = 0.0
+ self.v_ref[0:3, 0] = self.joystick.v_ref[0:3, 0] # TODO: Joystick velocity given in base frame and not
+ self.v_ref[3:6, 0] = self.joystick.v_ref[3:6, 0] # in horizontal frame (case of non flat ground)
+ self.v_ref[6:, 0] = 0.0
+
if not self.gait.getIsStatic():
# Integration to get evolution of perfect x, y and yaw
- self.q[3:7, 0] = self.estimator.EulerToQuaternion([0.0, 0.0, self.yaw_estim]) # Remove pitch and roll
- self.q[:, 0] = pin.integrate(self.solo.model, self.q, self.v_estim * self.myController.dt)
+ Ryaw = np.array([[np.cos(self.yaw_estim), -np.sin(self.yaw_estim)],
+ [np.sin(self.yaw_estim), np.cos(self.yaw_estim)]])
+ self.q[0:2, 0:1] = self.q[0:2, 0:1] + Ryaw @ self.v_ref[0:2, 0:1] * self.myController.dt
+ self.q[3:7, 0] = self.estimator.EulerToQuaternion([0.0, 0.0, self.yaw_estim + self.v_ref[5, 0:1] * self.myController.dt])
# Mix perfect x and y with height measurement
self.q[2, 0] = self.estimator.q_filt[2, 0]
@@ -230,7 +250,16 @@ class Controller:
# Velocities are the one estimated by the estimator
self.v = self.estimator.v_filt.copy()
+ quat = np.zeros((4, 1))
+ quat[:, 0] = self.estimator.EulerToQuaternion([self.RPY_filt[0], self.RPY_filt[1], 0.0])
+ hRb = pin.Quaternion(quat).toRotationMatrix()
+ self.h_v[0:3, 0:1] = hRb @ self.v[0:3, 0:1]
+ self.h_v[3:6, 0:1] = hRb @ self.v[3:6, 0:1]
+
+ # self.v[:6, 0] = self.joystick.v_ref[:6, 0]
else:
+ quat = np.array([[0.0, 0.0, 0.0, 1.0]]).transpose()
+ hRb = np.eye(3)
pass
# TODO: Adapt static mode to new version of the code
# self.planner.q_static[:] = pin.integrate(self.solo.model,
@@ -244,8 +273,64 @@ class Controller:
o_targetFootstep = self.footstepPlanner.updateFootsteps(self.k % self.k_mpc == 0 and self.k != 0,
int(self.k_mpc - self.k % self.k_mpc),
self.q[0:7, 0:1],
- self.v[0:6, 0:1].copy(),
- self.joystick.v_ref[0:6, 0])
+ self.h_v[0:6, 0:1].copy(),
+ self.v_ref[0:6, 0])
+
+ """
+ if self.k == 20:
+ self.joystick.v_ref[0, 0] = 0.1
+ N = 350
+ self.q = np.zeros((19, 1))
+ self.q[2, 0] = self.h_ref
+ self.q[7, 0] = 1.0
+ x_k = np.zeros((N - 20))
+ ctc = np.zeros((N - 20, 4))
+ ftp = np.zeros((N - 20, 4))
+ o_ftp = np.zeros((N - 20, 4))
+ h_ftg = np.zeros((N - 20, 4))
+ o_ftg = np.zeros((N - 20, 4))
+ while self.k < N:
+ self.k += 1
+
+ # Update gait
+ self.gait.updateGait(self.k, self.k_mpc, self.q[0:7, 0:1], self.joystick.joystick_code)
+
+ x_k[self.k - 21] = self.k
+ ctc[self.k - 21, :] = self.gait.getCurrentGait()[0, :]
+
+ # Compute target footstep based on current and reference velocities
+ o_targetFootstep = self.footstepPlanner.updateFootsteps(self.k % self.k_mpc == 0 and self.k != 0,
+ int(self.k_mpc - self.k % self.k_mpc),
+ self.q[0:7, 0:1],
+ self.v[0:6, 0:1].copy(),
+ self.joystick.v_ref[0:6, 0])
+
+ oRh = self.footstepPlanner.getRz()
+ oTh = np.array([[self.q[0, 0]], [self.q[1, 0]], [0.0]])
+
+ # Update pos, vel and acc references for feet
+ # TODO: Make update take as parameters current gait, swing phase duration and remaining time
+ self.footTrajectoryGenerator.update(self.k, o_targetFootstep)
+
+ if self.k % self.k_mpc == 0:
+ print(self.gait.getCurrentGait()[0:9, :])
+
+ o_ftg_k = self.footTrajectoryGenerator.getFootPosition()
+ h_ftg_k = oRh.transpose() @ (o_ftg_k - oTh)
+
+ for i in range(4):
+ o_ftp[self.k - 21, i] = o_targetFootstep[0, i]
+ h_ftg[self.k - 21, i] = h_ftg_k[0, i]
+ o_ftg[self.k - 21, i] = o_ftg_k[0, i]
+
+ self.q[0, 0] += 0.002 * self.joystick.v_ref[0, 0]
+
+ from matplotlib import pyplot as plt
+ plt.figure()
+ plt.plot(x_k, np.min(o_ftg[:, 0]) + (np.max(o_ftg[:, 0]) - np.min(o_ftg[:, 0])) * ctc[:, 0])
+ plt.plot(x_k, o_ftg[:, 0])
+ plt.show(block=True)
+ """
# Transformation matrices between world and horizontal frames
oRh = self.footstepPlanner.getRz()
@@ -256,14 +341,45 @@ class Controller:
self.footTrajectoryGenerator.update(self.k, o_targetFootstep)
# Run state planner (outputs the reference trajectory of the base)
- self.statePlanner.computeReferenceStates(self.q[0:7, 0:1], self.v[0:6, 0:1].copy(),
- self.joystick.v_ref[0:6, 0:1], 0.0)
+ self.statePlanner.computeReferenceStates(self.q[0:7, 0:1], self.h_v[0:6, 0:1].copy(),
+ self.v_ref[0:6, 0:1], 0.0)
# Result can be retrieved with self.statePlanner.getReferenceStates()
xref = self.statePlanner.getReferenceStates()
fsteps = self.footstepPlanner.getFootsteps()
cgait = self.gait.getCurrentGait()
+ pin.forwardKinematics(self.solo12_no_ff.model, self.solo12_no_ff.data, self.q[7:, 0:1], self.v[6:, 0:1])
+ pin.updateFramePlacements(self.solo12_no_ff.model, self.solo12_no_ff.data)
+ for i_ee in range(4):
+ idx = int(self.foot_ids_no_ff[i_ee])
+ self.b_pos_feet[:, i_ee] = self.solo12_no_ff.data.oMf[idx].translation
+ self.b_vel_feet[:, i_ee] = pin.getFrameVelocity(self.solo12_no_ff.model, self.solo12_no_ff.data, idx,
+ pin.LOCAL_WORLD_ALIGNED).linear
+
+ # Feet command acceleration in base frame
+ self.feet_a_cmd = oRh.transpose() @ self.footTrajectoryGenerator.getFootAcceleration() \
+ - np.cross(np.tile(self.v_ref[3:6, 0:1], (1, 4)), np.cross(np.tile(self.v_ref[3:6, 0:1], (1, 4)), self.feet_p_cmd, axis=0), axis=0) \
+ - 2 * np.cross(np.tile(self.v_ref[3:6, 0:1], (1, 4)), self.feet_v_cmd, axis=0)
+
+ # Feet command velocity in base frame
+ self.feet_v_cmd = oRh.transpose() @ self.footTrajectoryGenerator.getFootVelocity()
+ self.feet_v_cmd = self.feet_v_cmd - self.v_ref[0:3, 0:1] - np.cross(np.tile(self.v_ref[3:6, 0:1], (1, 4)), self.feet_p_cmd, axis=0)
+
+ # Feet command position in base frame
+ self.feet_p_cmd = oRh.transpose() @ (self.footTrajectoryGenerator.getFootPosition()
+ - np.array([[0.0], [0.0], [self.h_ref]]) - oTh)
+
+ """if self.k > 2541:
+ print(fsteps[:10, :])
+ print(oRh.transpose() @ (self.footTrajectoryGenerator.getFootPosition()[:, 0:1] - oTh))
+ from IPython import embed
+ embed()"""
+
+ """if self.k > 40:
+ from IPython import embed
+ embed()"""
+
t_planner = time.time()
# Solve MPC problem once every k_mpc iterations of the main loop
@@ -296,27 +412,44 @@ class Controller:
# If nothing wrong happened yet in the WBC controller
if (not self.myController.error) and (not self.joystick.stop):
+ """self.q_wbc = np.zeros((19, 1))
+ self.q_wbc[2, 0] = self.q[2, 0] # self.h_ref
+ self.q_wbc[3:7, 0] = quat[:, 0] # self.estimator.EulerToQuaternion([self.RPY_filt[0], self.RPY_filt[1], 0.0])
+ # self.q_wbc[6, 0] = 1.0
+ self.q_wbc[7:, 0] = self.q[7:, 0] # self.myController.qdes[7:]
+
+ # Get velocity in base frame for Pinocchio (not current base frame but desired base frame)
+ self.b_v = self.v.copy()
+ # self.b_v[:6, 0] = self.v_ref[:6, 0]
+ # self.b_v[0:3, 0:1] = hRb.transpose() @ self.v_ref[0:3, 0:1]
+ # self.b_v[3:6, 0:1] = hRb.transpose() @ self.v_ref[3:6, 0:1]
+ # self.b_v[6:, 0] = self.myController.vdes[6:, 0]"""
self.q_wbc = np.zeros((19, 1))
- self.q_wbc[2, 0] = self.q[2, 0]
- self.q_wbc[3:7, 0] = self.estimator.EulerToQuaternion([self.RPY_filt[0], self.RPY_filt[1], 0.0])
- self.q_wbc[7:, 0] = self.q[7:, 0]
+ self.q_wbc[2, 0] = self.h_ref # at position (0.0, 0.0, h_ref)
+ self.q_wbc[6, 0] = 1.0 # with orientation (0.0, 0.0, 0.0)
+ self.q_wbc[7:, 0] = self.myController.qdes[7:] # with reference angular positions of previous loop
- # Get velocity in base frame for Pinocchio
+ # Get velocity in base frame for Pinocchio (not current base frame but desired base frame)
self.b_v = self.v.copy()
+ self.b_v[:6, 0] = self.v_ref[:6, 0] # Base at reference velocity (TODO: add hRb once v_ref is considered in base frame)
+ self.b_v[6:, 0] = self.myController.vdes[6:, 0] # with reference angular velocities of previous loop
+
+ # self.b_v[0:3, 0:1] = hRb.transpose() @ self.v_ref[0:3, 0:1]
+ # self.b_v[3:6, 0:1] = hRb.transpose() @ self.v_ref[3:6, 0:1]
# Run InvKin + WBC QP
self.myController.compute(self.q_wbc, self.b_v, self.x_f_wbc[:12],
self.x_f_wbc[12:], cgait[0, :],
- oRh.transpose() @ (self.footTrajectoryGenerator.getFootPosition() - oTh),
- oRh.transpose() @ self.footTrajectoryGenerator.getFootVelocity(),
- oRh.transpose() @ self.footTrajectoryGenerator.getFootAcceleration())
+ self.feet_p_cmd,
+ self.feet_v_cmd,
+ self.feet_a_cmd)
# Quantities sent to the control board
self.result.P = 3.0 * np.ones(12)
self.result.D = 0.2 * np.ones(12)
self.result.q_des[:] = self.myController.qdes[7:]
self.result.v_des[:] = self.myController.vdes[6:, 0]
- self.result.tau_ff[:] = 0.5 * self.myController.tau_ff
+ self.result.tau_ff[:] = 0.8 * self.myController.tau_ff
# Display robot in Gepetto corba viewer
"""if self.k % 5 == 0:
@@ -328,7 +461,7 @@ class Controller:
self.security_check()
# Update PyBullet camera
- self.pyb_camera(device)
+ self.pyb_camera(device, self.RPY_filt[2])
# Logs
self.log_misc(t_start, t_filter, t_planner, t_mpc, t_wbc)
@@ -338,7 +471,7 @@ class Controller:
return 0.0
- def pyb_camera(self, device):
+ def pyb_camera(self, device, yaw):
# Update position of PyBullet camera on the robot position to do as if it was attached to the robot
if self.k > 10 and self.enable_pyb_GUI:
diff --git a/scripts/LoggerControl.py b/scripts/LoggerControl.py
index 7d87542f34cb0f6edff8a5d35b0e96c548412d5c..6f54286379f653a154c2d8cef312b1deb0165984 100644
--- a/scripts/LoggerControl.py
+++ b/scripts/LoggerControl.py
@@ -305,10 +305,10 @@ class LoggerControl():
"Task current state", "Task reference state"])"""
# Analysis of the footstep locations (current and future) with a slider to move along time
- self.slider_predicted_footholds()
+ # self.slider_predicted_footholds()
# Analysis of the footholds locations during the whole experiment
- import utils_mpc
+ """import utils_mpc
import pinocchio as pin
f_c = ["r", "b", "forestgreen", "rebeccapurple"]
quat = np.zeros((4, 1))
@@ -328,6 +328,7 @@ class LoggerControl():
# o_step[:, 0:1] = oRh @ steps[(j*3):((j+1)*3), 0:1] + self.loop_o_q_int[i:(i+1), 0:3].transpose()
o_step[:, 0:1] = oRh @ fsteps[0:1, (j*3):((j+1)*3)].transpose() + self.loop_o_q_int[i:(i+1), 0:3].transpose()
plt.plot(o_step[0, 0], o_step[1, 0], linestyle=None, linewidth=1, marker="o", color=f_c[j])
+ """
lgd1 = ["HAA", "HFE", "Knee"]
lgd2 = ["FL", "FR", "HL", "HR"]
diff --git a/scripts/solo12InvKin.py b/scripts/solo12InvKin.py
index 57fd4580365b5d12e440d35ba5af2e9340c5f073..76746818676814b207b75fa7798fc364cafb4d00 100644
--- a/scripts/solo12InvKin.py
+++ b/scripts/solo12InvKin.py
@@ -1,13 +1,15 @@
-from example_robot_data import load
+# from example_robot_data import load
import numpy as np
import pinocchio as pin
import libquadruped_reactive_walking as lrw
-
+from example_robot_data.robots_loader import Solo12Loader
class Solo12InvKin:
def __init__(self, dt):
- self.robot = load('solo12')
+ # self.robot = load('solo12')
+ Solo12Loader.free_flyer = False
+ self.robot = Solo12Loader().robot
self.dt = dt
self.InvKinCpp = lrw.InvKin(dt)
@@ -49,13 +51,13 @@ class Solo12InvKin:
self.cpp_vf = np.zeros((4, 3))
self.cpp_wf = np.zeros((4, 3))
self.cpp_af = np.zeros((4, 3))
- self.cpp_Jf = np.zeros((12, 18))
+ self.cpp_Jf = np.zeros((12, 12))
self.cpp_posb = np.zeros((1, 3))
self.cpp_rotb = np.zeros((3, 3))
self.cpp_vb = np.zeros((1, 6))
self.cpp_ab = np.zeros((1, 6))
- self.cpp_Jb = np.zeros((6, 18))
+ self.cpp_Jb = np.zeros((6, 12))
self.cpp_ddq = np.zeros((18,))
self.cpp_q_cmd = np.zeros((19,))
@@ -66,7 +68,7 @@ class Solo12InvKin:
FR_FOOT_ID = self.robot.model.getFrameId('FR_FOOT')
HL_FOOT_ID = self.robot.model.getFrameId('HL_FOOT')
HR_FOOT_ID = self.robot.model.getFrameId('HR_FOOT')
- self.BASE_ID = self.robot.model.getFrameId('base_link')
+ self.BASE_ID = FL_FOOT_ID # self.robot.model.getFrameId('base_link') TODO REMOVE
self.foot_ids = np.array([FL_FOOT_ID, FR_FOOT_ID, HL_FOOT_ID, HR_FOOT_ID])
def dinv(J, damping=1e-2):
@@ -133,12 +135,12 @@ class Solo12InvKin:
self.cpp_ab[0, 3:6] = acc.angular
self.cpp_Jb[:, :] = pin.getFrameJacobian(self.robot.model, self.robot.data, self.BASE_ID, pin.LOCAL_WORLD_ALIGNED)
- self.cpp_ddq[:] = self.InvKinCpp.refreshAndCompute(np.array([x_cmd]), np.array([contacts]), pgoals, vgoals, agoals,
- self.cpp_posf, self.cpp_vf, self.cpp_wf, self.cpp_af, self.cpp_Jf,
- self.cpp_posb, self.cpp_rotb, self.cpp_vb, self.cpp_ab, self.cpp_Jb)
+ self.cpp_ddq[6:] = self.InvKinCpp.refreshAndCompute(np.array([x_cmd]), np.array([contacts]), pgoals, vgoals, agoals,
+ self.cpp_posf, self.cpp_vf, self.cpp_wf, self.cpp_af, self.cpp_Jf,
+ self.cpp_posb, self.cpp_rotb, self.cpp_vb, self.cpp_ab, self.cpp_Jb)
- self.cpp_q_cmd[:] = pin.integrate(self.robot.model, q, self.InvKinCpp.get_q_step())
- self.cpp_dq_cmd[:] = self.InvKinCpp.get_dq_cmd()
+ self.cpp_q_cmd[7:] = q[:, 0] + self.InvKinCpp.get_q_step() # pin.integrate(self.robot.model, q, self.InvKinCpp.get_q_step())
+ self.cpp_dq_cmd[6:] = self.InvKinCpp.get_dq_cmd()
self.q_cmd = self.cpp_q_cmd
self.dq_cmd = self.cpp_dq_cmd
diff --git a/src/InvKin.cpp b/src/InvKin.cpp
index b58cd6c9ba5be8a37b18932276a574126c6eac21..c0c2191db6ffd9308fd300fb37772e1781f2803d 100644
--- a/src/InvKin.cpp
+++ b/src/InvKin.cpp
@@ -63,12 +63,13 @@ Eigen::MatrixXd InvKin::computeInvKin(const Eigen::MatrixXd &posf, const Eigen::
afeet.row(i) = + Kp_flyingfeet * pfeet_err.row(i) - Kd_flyingfeet * (vf.row(i)-feet_velocity_ref.row(i)) + feet_acceleration_ref.row(i);
if (flag_in_contact(0, i)) {
- afeet.row(i) *= 1.0; // Set to 0.0 to disable position/velocity control of feet in contact
+ afeet.row(i) *= 0.0; // Set to 0.0 to disable position/velocity control of feet in contact
}
afeet.row(i) -= af.row(i) + cross3(wf.row(i), vf.row(i)); // Drift
}
- J.block(6, 0, 12, 18) = Jf.block(0, 0, 12, 18);
+ J.block(0, 0, 12, 12) = Jf.block(0, 0, 12, 12);
+ /*
// Process base position
e_basispos = base_position_ref - posb;
abasis = Kp_base_position * e_basispos - Kd_base_position * (vb.block(0, 0, 1, 3) - base_linearvelocity_ref);
@@ -89,28 +90,31 @@ Eigen::MatrixXd InvKin::computeInvKin(const Eigen::MatrixXd &posf, const Eigen::
x_ref.block(3, 0, 3, 1) = vb.block(0, 3, 1, 3);
J.block(0, 0, 6, 18) = Jb.block(0, 0, 6, 18); // Position and orientation
+ */
- acc.block(0, 0, 1, 3) = abasis;
- acc.block(0, 3, 1, 3) = awbasis;
+ //acc.block(0, 0, 1, 3) = abasis;
+ //acc.block(0, 3, 1, 3) = awbasis;
for (int i = 0; i < 4; i++) {
- acc.block(0, 6+3*i, 1, 3) = afeet.row(i);
+ acc.block(0, 3*i, 1, 3) = afeet.row(i);
}
- x_err.block(0, 0, 1, 3) = e_basispos;
- x_err.block(0, 3, 1, 3) = e_basisrot;
+ //x_err.block(0, 0, 1, 3) = e_basispos;
+ //x_err.block(0, 3, 1, 3) = e_basisrot;
for (int i = 0; i < 4; i++) {
- x_err.block(0, 6+3*i, 1, 3) = pfeet_err.row(i);
+ x_err.block(0, 3*i, 1, 3) = pfeet_err.row(i);
}
- dx_r.block(0, 0, 1, 3) = base_linearvelocity_ref;
- dx_r.block(0, 3, 1, 3) = base_angularvelocity_ref;
+ //dx_r.block(0, 0, 1, 3) = base_linearvelocity_ref;
+ //dx_r.block(0, 3, 1, 3) = base_angularvelocity_ref;
for (int i = 0; i < 4; i++) {
- dx_r.block(0, 6+3*i, 1, 3) = vfeet_ref.row(i);
+ dx_r.block(0, 3*i, 1, 3) = vfeet_ref.row(i);
}
// std::cout << "J" << std::endl << J << std::endl;
- invJ = pseudoInverse(J);
+ for (int i = 0; i < 4; i++) {
+ invJ.block(3*i, 3*i, 3, 3) = J.block(3*i, 3*i, 3, 3).inverse();
+ }
// std::cout << "invJ" << std::endl << invJ << std::endl;
// std::cout << "acc" << std::endl << acc << std::endl;