mirror of
https://github.com/wassname/pyrobolearn.git
synced 2026-09-09 11:31:38 +08:00
update robot: add centroidal dynamics and few minor updates
This commit is contained in:
Brian Delhaisse
+416
-141
@@ -25,7 +25,7 @@ from pyrobolearn.robots.base import ControllableBody
|
||||
|
||||
__author__ = "Brian Delhaisse"
|
||||
__copyright__ = "Copyright 2018, PyRoboLearn"
|
||||
__credits__ = ["Brian Delhaisse", "Leonel Rozo"]
|
||||
__credits__ = ["Brian Delhaisse", "Leonel Rozo", "Songyan Xin"]
|
||||
__license__ = "MIT"
|
||||
__version__ = "1.0.0"
|
||||
__maintainer__ = "Brian Delhaisse"
|
||||
@@ -182,7 +182,16 @@ class Robot(ControllableBody):
|
||||
|
||||
@property
|
||||
def num_dofs(self):
|
||||
"""Return the number of degrees of freedom (i.e. the number of joints that are not fixed)"""
|
||||
"""Return the number of degrees of freedom (DoFs); that is, if the base is not fixed, 6 (= 3 degrees for
|
||||
translation + 3 degrees for orientation) + the joints that are not fixed.
|
||||
"""
|
||||
if self.fixed_base:
|
||||
return len(self.joints)
|
||||
return len(self.joints) + 6
|
||||
|
||||
@property
|
||||
def num_free_joints(self):
|
||||
"""Return the number of joints that are not fixed."""
|
||||
return len(self.joints)
|
||||
|
||||
########
|
||||
@@ -304,6 +313,9 @@ class Robot(ControllableBody):
|
||||
self.com = self.sim.get_center_of_mass_position(self.id)
|
||||
return self.com
|
||||
|
||||
# alias
|
||||
get_com_position = get_center_of_mass_position
|
||||
|
||||
def get_center_of_mass_velocity(self):
|
||||
"""
|
||||
Return the center of mass velocity.
|
||||
@@ -313,6 +325,9 @@ class Robot(ControllableBody):
|
||||
"""
|
||||
return self.sim.get_center_of_mass_velocity(self.id)
|
||||
|
||||
# alias
|
||||
get_com_velocity = get_center_of_mass_velocity
|
||||
|
||||
# def get_linear_momentum(self):
|
||||
# """
|
||||
# Compute the linear momentum around the center of mass.
|
||||
@@ -339,28 +354,6 @@ class Robot(ControllableBody):
|
||||
# np.array[3]: angular momentum
|
||||
# """
|
||||
# pass
|
||||
#
|
||||
# def get_centroidal_dynamics(self, q=None, dq=None):
|
||||
# """
|
||||
# Compute the centroidal momentum dynamics based on [1]. "The centroidal momentum of a rigid-body system
|
||||
# consists of its net linear momentum as well as its net angular momentum about its center of mass (CoM)" [1]
|
||||
#
|
||||
# Args:
|
||||
# q (np.array[N], None): joint positions of size N, where N is the total number of DoFs. If None, it will
|
||||
# get the current joint positions (but note that this could lead to a decrease of performance).
|
||||
# dq (np.array[M], None): joint velocities of size M (with 0 < M <= N). If None, it will
|
||||
# get the current joint velocities (but note that this could lead to a decrease of performance).
|
||||
#
|
||||
# Returns:
|
||||
# np.array[6, N+6]: centroidal momentum matrix :math:`A_G`
|
||||
# np.array[6]: the dot product between the derivative of the centroidal momentum matrix with the
|
||||
# generalized velocities vector. That is, :math:`\dot{A}_G \dot{q}`
|
||||
#
|
||||
# References:
|
||||
# [1] "Improved computation of the humanoid centroidal dynamics and application for whole-body control",
|
||||
# Wensing and Orin, 2016
|
||||
# """
|
||||
# pass
|
||||
|
||||
########################
|
||||
# Joints (joint space) #
|
||||
@@ -1520,7 +1513,7 @@ class Robot(ControllableBody):
|
||||
link_ids = list(range(self.num_links))
|
||||
return np.array([self.sim.get_dynamics_info(self.id, link)[2] for link in link_ids])
|
||||
|
||||
def set_link_positions(self, link_ids, position, orientation=None):
|
||||
def set_link_positions(self, link_ids, positions, orientations=None):
|
||||
"""
|
||||
Set the position(s) of the given link(s) using inverse kinematics (IK).
|
||||
|
||||
@@ -1529,9 +1522,22 @@ class Robot(ControllableBody):
|
||||
|
||||
Args:
|
||||
link_ids (int, int[N]): link id, or list of desired link ids.
|
||||
position (np.array[3], [np.array[3]], np.array[N,3]):
|
||||
orientation (np.array[4], [np.array[4]], np.array[N,4]):
|
||||
positions (np.array[3], [np.array[3]], np.array[N,3]): desired link position(s).
|
||||
orientations (np.array[4], list of np.array[4], np.array[N,4]): desired link orientation(s) (expressed as
|
||||
quaternions [x,y,z,w])
|
||||
"""
|
||||
# TODO: think when setting the position of multiple links where some joints are shared (need to use the
|
||||
# null-space)
|
||||
pass
|
||||
|
||||
def set_link_velocities(self, link_ids, velocities):
|
||||
# TODO: think when setting the position of multiple links where some joints are shared (need to use the
|
||||
# null-space)
|
||||
pass
|
||||
|
||||
def set_link_forces(self, link_ids, forces):
|
||||
# TODO: think when setting the position of multiple links where some joints are shared (need to use the
|
||||
# null-space)
|
||||
pass
|
||||
|
||||
#################
|
||||
@@ -1869,10 +1875,10 @@ class Robot(ControllableBody):
|
||||
robot_mass = 0
|
||||
|
||||
# initialize center of mass jacobian: J_com
|
||||
if self.has_fixed_base():
|
||||
Jcom = np.zeros((6, self.num_dofs))
|
||||
else:
|
||||
Jcom = np.zeros((6, self.num_dofs + 6))
|
||||
# if self.has_fixed_base():
|
||||
Jcom = np.zeros((6, self.num_dofs))
|
||||
# else:
|
||||
# Jcom = np.zeros((6, self.num_dofs + 6))
|
||||
|
||||
# calculate the CoM jacobian
|
||||
for link_id in range(self.num_links):
|
||||
@@ -1885,6 +1891,9 @@ class Robot(ControllableBody):
|
||||
|
||||
return Jcom
|
||||
|
||||
# alias
|
||||
get_com_jacobian = get_center_of_mass_jacobian
|
||||
|
||||
def get_angular_velocities_from_derivative_rpy(self, rpy_angle, dRPY):
|
||||
r"""
|
||||
Return the angular velocities :math:`\omega` from the derivative of RPY Euler angles \math:`\dot{\phi}`.
|
||||
@@ -2041,21 +2050,6 @@ class Robot(ControllableBody):
|
||||
"""
|
||||
return np.linalg.inv(self.get_JJT(jacobian))
|
||||
|
||||
@staticmethod
|
||||
def compute_dynamic_manipulability_ellipsoid(jacobian, inertia):
|
||||
r"""
|
||||
Compute the dynamic manipulability ellipsoid (matrix) as `M = J(q)H(q)^{-1} (J(q)H(q)^{-1})^T`.
|
||||
|
||||
Args:
|
||||
jacobian (np.array[D,N]): Jacobian matrix
|
||||
inertia (np.array[N,N], np.array[6+N,6+N], np.array[M,M]): inertia matrix in joint space
|
||||
|
||||
Returns:
|
||||
np.array[D,D]: dynamic manipulability
|
||||
"""
|
||||
epsilon = jacobian.dot(np.linalg.inv(inertia))
|
||||
return epsilon.dot(epsilon.T)
|
||||
|
||||
@staticmethod
|
||||
def in_singular_configuration(jacobian):
|
||||
r"""
|
||||
@@ -2194,44 +2188,6 @@ class Robot(ControllableBody):
|
||||
|
||||
return dq, np.min(S), distance
|
||||
|
||||
def calculate_inverse_differential_kinematics_dynamic_manipulability(self, jacobian, inertia,
|
||||
target_dynamic_manipulability, Km):
|
||||
"""
|
||||
Compute the inverse differential kinematics for dynamic Manipulability; it will return a joint velocity for all
|
||||
the actuated joints.
|
||||
|
||||
Args:
|
||||
jacobian (np.array[D,N]): Jacobian matrix
|
||||
target_dynamic_manipulability (np.array[D,D]): target dynamic manipulability
|
||||
Km (float[,]): Proportional gain for manipulability error
|
||||
|
||||
Returns:
|
||||
np.array[N]: joint velocities
|
||||
float: minimum of eigenvalues of the dynamic manip. Jacobian
|
||||
float: Distance between desired and current manip. ellipsoids
|
||||
"""
|
||||
num_task_vars = np.size(target_dynamic_manipulability, 0)
|
||||
|
||||
# Compute manipulability error
|
||||
dynamic_manip = self.compute_dynamic_manipulability_ellipsoid(jacobian, inertia)
|
||||
Me = logarithm_map([target_dynamic_manipulability[0:num_task_vars, 0:num_task_vars]],
|
||||
dynamic_manip[0:num_task_vars, 0:num_task_vars])[0]
|
||||
# print("Me: {}".format(Me))
|
||||
distance = distance_spd(target_dynamic_manipulability[0:num_task_vars, 0:num_task_vars],
|
||||
dynamic_manip[0:num_task_vars, 0:num_task_vars])
|
||||
print("SPD dist: {}".format(distance))
|
||||
|
||||
Jm_red = self.compute_dynamic_manipulability_jacobian(jacobian, inertia, num_task_vars)
|
||||
# print("Jm: {}".format(Jm_red))
|
||||
|
||||
# Matrix singularity robustness
|
||||
U, S, Vh = np.linalg.svd(Jm_red)
|
||||
damping = 1E-2 if np.min(S) < 1E-2 else 1E-8
|
||||
|
||||
dq = np.dot(self.get_damped_least_squares_inverse(Jm_red, damping), np.dot(Km, symmetric_matrix_to_vector(Me)))
|
||||
|
||||
return dq, np.min(S), distance
|
||||
|
||||
def compute_velocity_manipulability_jacobian(self, jacobian, num_task_vars):
|
||||
r"""
|
||||
Compute the velocity manipulability Jacobian [1].
|
||||
@@ -2274,50 +2230,6 @@ class Robot(ControllableBody):
|
||||
|
||||
return Jm_red
|
||||
|
||||
def compute_dynamic_manipulability_jacobian(self, jacobian, inertia, num_task_vars):
|
||||
r"""
|
||||
Compute the dynamic manipulability Jacobian.
|
||||
|
||||
Args:
|
||||
jacobian (np.array[D,N]): Jacobian matrix
|
||||
inertia (np.array[N,N]): inertia matrix
|
||||
num_task_vars (float): number of task variables (usually 3 or 6)
|
||||
|
||||
Returns:
|
||||
np.array[(num_task_vars * num_task_vars + num_task_vars) / 2, N]: manipulability jacobian matrix
|
||||
"""
|
||||
num_dofs = jacobian.shape[1]
|
||||
|
||||
# Compute derivative of Jacobian wrt joint angles
|
||||
J_grad = self.compute_jacobian_joint_derivative(jacobian)
|
||||
# Compute derivative of Inertia matrix wrt joint angles
|
||||
H_grad = self.compute_inertia_joint_derivative(jacobian)
|
||||
#
|
||||
L = np.dot(jacobian, np.linalg.inv(inertia))
|
||||
|
||||
# for i in range(J_grad.shape[2]):
|
||||
# print("dJ/dq_{}: {}".format(i, J_grad[:, :, i]))
|
||||
# for i in range(H_grad.shape[2]):
|
||||
# print("dH/dq_{}: {}".format(i, H_grad[:, :, i]))
|
||||
|
||||
# Dynamic manipulability Jacobian
|
||||
Lgrad = tensor_matrix_product(J_grad, np.linalg.inv(inertia), 1) - \
|
||||
tensor_matrix_product(tensor_matrix_product(H_grad, L, 0), np.linalg.inv(inertia), 1)
|
||||
Jm = tensor_matrix_product(np.transpose(Lgrad[:, :], [1, 0, 2]), L, 0) + tensor_matrix_product(Lgrad, L, 1)
|
||||
# for i in range(Jm.shape[2]):
|
||||
# print("Jm_{}: {}".format(i, Jm[:, :, i]))
|
||||
# Jm = Jm[num_task_vars, num_task_vars, :]
|
||||
|
||||
# # Manipulability Jacobian in matrix form (Mandel notation)
|
||||
# num_vars = len(num_task_vars)
|
||||
Jm_red = np.zeros(((num_task_vars * num_task_vars + num_task_vars) / 2, np.sum(num_dofs)))
|
||||
# print("Jm_red.shape: {}".format(Jm_red.shape))
|
||||
|
||||
for i in range(Jm.shape[2]):
|
||||
Jm_red[:, i] = symmetric_matrix_to_vector(Jm[0:num_task_vars, 0:num_task_vars, i])
|
||||
|
||||
return Jm_red
|
||||
|
||||
def hard_priorities(self, jacobians, task_velocities, method='backtrack'):
|
||||
r"""
|
||||
Return dq.
|
||||
@@ -2472,7 +2384,7 @@ class Robot(ControllableBody):
|
||||
if q is None:
|
||||
q = self.get_joint_positions()
|
||||
else:
|
||||
if len(q) != self.num_dofs:
|
||||
if len(q) != self.num_free_joints: # self.num_dofs:
|
||||
raise ValueError("All the joint positions need to be given to this method. You can then slice the"
|
||||
"inertia matrix afterward.")
|
||||
|
||||
@@ -2485,6 +2397,9 @@ class Robot(ControllableBody):
|
||||
return np.array(self.sim.calculate_mass_matrix(self.id, q_aug))
|
||||
return np.array(self.sim.calculate_mass_matrix(self.id, q_aug))[q_idx, q_idx]
|
||||
|
||||
# alias
|
||||
get_inertia_matrix = get_mass_matrix
|
||||
|
||||
def compute_inertia_joint_derivative(self, jacobian):
|
||||
r"""
|
||||
Compute the derivative of the Inertia matrix H(q) wrt joint values q.
|
||||
@@ -2749,6 +2664,46 @@ class Robot(ControllableBody):
|
||||
dq = np.zeros(len(q))
|
||||
return self.get_coriolis_and_gravity_compensation_torques(q, dq, q_idx)
|
||||
|
||||
def get_coriolis_torques(self, q=None, dq=None, q_idx=None):
|
||||
r"""
|
||||
Return the torques that need to be applied to the robot joints such that it compensates for Coriolis effects
|
||||
in the absence of gravity, i.e. :math:`\tau = C(q,\dot{q}) \dot{q}`
|
||||
|
||||
From the equations of motion:
|
||||
|
||||
.. math:: H(q) \ddot{q} + C(q,\dot{q}) \dot{q} + g(q) = \tau + J^T(q) F,
|
||||
|
||||
we can see that if we set :math:`F` and :math:`\ddot{q}` to 0, then we have:
|
||||
|
||||
.. math:: \tau_1 = C(q,\dot{q}) \dot{q} + g(q),
|
||||
|
||||
and if additionally, we set :math:`\dot{q}` to 0, then we have:
|
||||
|
||||
.. math:: \tau_2 = g(q).
|
||||
|
||||
We can then get :math:`C(q,\dot{q}) \dot{q} = \tau_1 - \tau_2`.
|
||||
|
||||
Args:
|
||||
q (np.array[N], None): all the joint positions. If None, it will get the current joint positions of all the
|
||||
joints. However, note that if you already got the joint positions in your code,
|
||||
it is better to pass them to this method for performance.
|
||||
dq (np.array[N], None): all the joint velocities. If None, it will get the current joint velocities of
|
||||
all the joints.
|
||||
q_idx (int[M], None): slice the torques at the given q indices (0 < M <= N).
|
||||
|
||||
Returns:
|
||||
np.array[M]: joint torques to be applied [Nm]
|
||||
"""
|
||||
if q is None:
|
||||
q = self.get_joint_positions()
|
||||
if dq is None:
|
||||
dq = self.get_joint_velocities()
|
||||
|
||||
tau1 = self.get_coriolis_and_gravity_compensation_torques(q=q, dq=dq, q_idx=q_idx)
|
||||
tau2 = self.get_gravity_compensation_torques(q=q, q_idx=q_idx)
|
||||
|
||||
return tau1 - tau2
|
||||
|
||||
def apply_coriolis_and_gravity_compensation(self, q=None, dq=None, q_idx=None, external_torques=0.):
|
||||
r"""
|
||||
Apply Coriolis and Gravity Compensation; set the torques using torque control.
|
||||
@@ -2772,7 +2727,7 @@ class Robot(ControllableBody):
|
||||
|
||||
# TODO: finish to implement the method + think about multiple links + think about dimensions
|
||||
def get_active_compliant_torques(self, q=None, dq=None, q_idx=None, jacobian=None, link_velocity=None,
|
||||
link_ids=None, kd=60):
|
||||
link_id=None, kd=60):
|
||||
r"""
|
||||
Return the torques that need to be applied to enable active compliance. This is done by enabling Coriolis
|
||||
and gravity compensation along with a damping force projected from the Cartesian space to the joint space.
|
||||
@@ -2790,6 +2745,8 @@ class Robot(ControllableBody):
|
||||
dq (np.array[N], None): all the joint velocities. If None, it will get the current joint velocities of
|
||||
all the joints.
|
||||
q_idx (int[M], None): slice the torques at the given q indices (0 < M <= N).
|
||||
jacobian (np.array[6,N], np.array[6,6+N]): Jacobian matrix.
|
||||
link_velocity (np.array[6]): linear and angular velocity of the link in the Cartesian world space
|
||||
|
||||
Returns:
|
||||
np.array[M]: joint torques to be applied [Nm]
|
||||
@@ -2822,33 +2779,341 @@ class Robot(ControllableBody):
|
||||
dq (np.array[N], None): all the joint velocities. If None, it will get the current joint velocities of
|
||||
all the joints.
|
||||
q_idx (int[M], None): slice the torques at the given q indices (0 < M <= N).
|
||||
external_torques (float, np.array[M]): external torques.
|
||||
"""
|
||||
joint_id = self.joints if q_idx is None else self.joints[q_idx]
|
||||
torques = self.get_active_compliant_torques(q, dq, q_idx)
|
||||
self.set_joint_torques(joint_id, torques + external_torques)
|
||||
self.set_joint_torques(torques + external_torques, joint_id)
|
||||
|
||||
def get_impedance_torques(self, x=0, dx=0, ddx=0):
|
||||
def get_impedance_torques(self, jacobian, xdes=0, x=0, dx=0, dxdes=0, ddx=0, ddxdes=0, Km=1, Dm=0.01):
|
||||
r"""
|
||||
Return the impedance torques.
|
||||
|
||||
.. math:: F_{a} = H_m (\ddot{x} - \ddot{x}_d) + D_m (\dot{x} - \dot{x}_d) + K_m (x - x_d)
|
||||
|
||||
Args:
|
||||
|
||||
|
||||
Returns:
|
||||
np.array[N]: impedance torques
|
||||
|
||||
References:
|
||||
[1] Lecture on "Impedance Control" by Prof. De Luca, Universita di Roma,
|
||||
http://www.diag.uniroma1.it/~deluca/rob2_en/15_ImpedanceControl.pdf
|
||||
"""
|
||||
pass
|
||||
|
||||
def apply_task_impedance_control(self):
|
||||
def get_attractor_torques(self, x_des, jacobian, link_id=None, q=None, dq=None, x=None, dx=None, K=5, D=0.1):
|
||||
r"""
|
||||
.. math:: F_{a} = H_m (\ddot{x} - \ddot{x}_d) + D_m (\dot{x} - \dot{x}_d) + K_m (x - x_d)
|
||||
"""
|
||||
pass
|
||||
|
||||
def get_attractor_torques(self):
|
||||
r"""
|
||||
The torques to be applied are given by:
|
||||
The torques to be applied (using impedance control with an attractor point) are given by:
|
||||
|
||||
.. math:: \tau = C(q,\dot{q}) \dot{q} + g(q) + J^T F
|
||||
|
||||
where :math:`F = K(x_d - x) - D v` with :math:`x` and :math:`v` are the Cartesian position and velocities,
|
||||
and :math:`K` and :math:`D` are the stiffness and damping factor, respectively.
|
||||
|
||||
Args:
|
||||
q (np.array[N]): joint positions
|
||||
dq (np.array[N]): joint velocities
|
||||
x_des (np.array[3]): desired position of the link
|
||||
x (np.array[3]): cartesian world position of the link
|
||||
dx (np.array[3]): cartesian world linear velocity of the link
|
||||
jacobian (np.array[3,N]): linear jacobian associated to the link
|
||||
K (float, np.array[3,3]): proportional gain scalar or matrix
|
||||
D (float, np.array[3,3]): derivative gain scalar or matrix
|
||||
|
||||
Returns:
|
||||
np.array[N]: torques to apply
|
||||
|
||||
References:
|
||||
[1] Lecture on "Impedance Control" by Prof. De Luca, Universita di Roma,
|
||||
http://www.diag.uniroma1.it/~deluca/rob2_en/15_ImpedanceControl.pdf
|
||||
"""
|
||||
pass
|
||||
# check arguments
|
||||
if q is None:
|
||||
q = self.get_joint_positions()
|
||||
if dq is None:
|
||||
dq = self.get_joint_velocities()
|
||||
if link_id is not None:
|
||||
if x is None:
|
||||
x = self.get_link_world_positions(link_id)
|
||||
if dx is None:
|
||||
dx = self.get_link_world_linear_velocities(link_id)
|
||||
if jacobian is None:
|
||||
jacobian = self.get_jacobian(link_id, q=q)
|
||||
|
||||
# coriolis and gravity torques
|
||||
torques = self.get_coriolis_and_gravity_compensation_torques(q=q, dq=dq)
|
||||
|
||||
# impedance control: attractor point
|
||||
forces = K.dot(x_des - x) - D.dot(dx)
|
||||
torques += jacobian.T.dot(forces)
|
||||
|
||||
return torques
|
||||
|
||||
@staticmethod
|
||||
def compute_dynamic_manipulability_ellipsoid(jacobian, inertia):
|
||||
r"""
|
||||
Compute the dynamic manipulability ellipsoid (matrix) as `M = J(q)H(q)^{-1} (J(q)H(q)^{-1})^T`.
|
||||
|
||||
Args:
|
||||
jacobian (np.array[D,N]): Jacobian matrix
|
||||
inertia (np.array[N,N]): inertia matrix in joint space
|
||||
|
||||
Returns:
|
||||
np.array[D,D]: dynamic manipulability
|
||||
"""
|
||||
epsilon = jacobian.dot(np.linalg.inv(inertia))
|
||||
return epsilon.dot(epsilon.T)
|
||||
|
||||
def calculate_inverse_differential_kinematics_dynamic_manipulability(self, jacobian, inertia,
|
||||
target_dynamic_manipulability, Km):
|
||||
"""
|
||||
Compute the inverse differential kinematics for dynamic Manipulability; it will return a joint velocity for all
|
||||
the actuated joints.
|
||||
|
||||
Args:
|
||||
jacobian (np.array[D,N]): Jacobian matrix
|
||||
inertia (np.array[N,N]): inertia matrix
|
||||
target_dynamic_manipulability (np.array[D,D]): target dynamic manipulability
|
||||
Km (float[,]): Proportional gain for manipulability error
|
||||
|
||||
Returns:
|
||||
np.array[N]: joint velocities
|
||||
float: minimum of eigenvalues of the dynamic manip. Jacobian
|
||||
float: Distance between desired and current manip. ellipsoids
|
||||
"""
|
||||
num_task_vars = np.size(target_dynamic_manipulability, 0)
|
||||
|
||||
# Compute manipulability error
|
||||
dynamic_manip = self.compute_dynamic_manipulability_ellipsoid(jacobian, inertia)
|
||||
Me = logarithm_map([target_dynamic_manipulability[0:num_task_vars, 0:num_task_vars]],
|
||||
dynamic_manip[0:num_task_vars, 0:num_task_vars])[0]
|
||||
# print("Me: {}".format(Me))
|
||||
distance = distance_spd(target_dynamic_manipulability[0:num_task_vars, 0:num_task_vars],
|
||||
dynamic_manip[0:num_task_vars, 0:num_task_vars])
|
||||
print("SPD dist: {}".format(distance))
|
||||
|
||||
Jm_red = self.compute_dynamic_manipulability_jacobian(jacobian, inertia, num_task_vars)
|
||||
# print("Jm: {}".format(Jm_red))
|
||||
|
||||
# Matrix singularity robustness
|
||||
U, S, Vh = np.linalg.svd(Jm_red)
|
||||
damping = 1E-2 if np.min(S) < 1E-2 else 1E-8
|
||||
|
||||
dq = np.dot(self.get_damped_least_squares_inverse(Jm_red, damping), np.dot(Km, symmetric_matrix_to_vector(Me)))
|
||||
|
||||
return dq, np.min(S), distance
|
||||
|
||||
def compute_dynamic_manipulability_jacobian(self, jacobian, inertia, num_task_vars):
|
||||
r"""
|
||||
Compute the dynamic manipulability Jacobian.
|
||||
|
||||
Args:
|
||||
jacobian (np.array[D,N]): Jacobian matrix
|
||||
inertia (np.array[N,N]): inertia matrix
|
||||
num_task_vars (float): number of task variables (usually 3 or 6)
|
||||
|
||||
Returns:
|
||||
np.array[(num_task_vars * num_task_vars + num_task_vars) / 2, N]: manipulability jacobian matrix
|
||||
"""
|
||||
num_dofs = jacobian.shape[1]
|
||||
|
||||
# Compute derivative of Jacobian wrt joint angles
|
||||
J_grad = self.compute_jacobian_joint_derivative(jacobian)
|
||||
# Compute derivative of Inertia matrix wrt joint angles
|
||||
H_grad = self.compute_inertia_joint_derivative(jacobian)
|
||||
#
|
||||
L = np.dot(jacobian, np.linalg.inv(inertia))
|
||||
|
||||
# for i in range(J_grad.shape[2]):
|
||||
# print("dJ/dq_{}: {}".format(i, J_grad[:, :, i]))
|
||||
# for i in range(H_grad.shape[2]):
|
||||
# print("dH/dq_{}: {}".format(i, H_grad[:, :, i]))
|
||||
|
||||
# Dynamic manipulability Jacobian
|
||||
Lgrad = tensor_matrix_product(J_grad, np.linalg.inv(inertia), 1) - \
|
||||
tensor_matrix_product(tensor_matrix_product(H_grad, L, 0), np.linalg.inv(inertia), 1)
|
||||
Jm = tensor_matrix_product(np.transpose(Lgrad[:, :], [1, 0, 2]), L, 0) + tensor_matrix_product(Lgrad, L, 1)
|
||||
# for i in range(Jm.shape[2]):
|
||||
# print("Jm_{}: {}".format(i, Jm[:, :, i]))
|
||||
# Jm = Jm[num_task_vars, num_task_vars, :]
|
||||
|
||||
# # Manipulability Jacobian in matrix form (Mandel notation)
|
||||
# num_vars = len(num_task_vars)
|
||||
Jm_red = np.zeros(((num_task_vars * num_task_vars + num_task_vars) / 2, np.sum(num_dofs)))
|
||||
# print("Jm_red.shape: {}".format(Jm_red.shape))
|
||||
|
||||
for i in range(Jm.shape[2]):
|
||||
Jm_red[:, i] = symmetric_matrix_to_vector(Jm[0:num_task_vars, 0:num_task_vars, i])
|
||||
|
||||
return Jm_red
|
||||
|
||||
def get_centroidal_dynamics(self, q=None, dq=None, inertia=None):
|
||||
r"""
|
||||
Compute the centroidal momentum dynamics based on [1]. "The centroidal momentum of a rigid-body system
|
||||
consists of its net linear momentum as well as its net angular momentum about its center of mass (CoM)" [1]
|
||||
|
||||
The centroidal momentum, which is the sum of all body spatial momenta computed with respect to the CoM, is
|
||||
given by:
|
||||
|
||||
.. math:: h_G = A_G \dot{q},
|
||||
|
||||
where :math:`h_G \in \mathbb{R}^6` is the centroidal momentum, :math:`A_G \in \mathbb{R}^{6 \times (n+6)}` is
|
||||
the centroidal momentum matrix (CMM), and :math:`\dot{q}` are the system's generalized velocities. The CMM is
|
||||
related to the joint space inertia matrix (see code). This centroidal momentum collects the system linear and
|
||||
angular momentum together.
|
||||
|
||||
The centroidal dynamics are then given by the equation:
|
||||
|
||||
.. math:: \dot{h}_G = A_G \ddot{q} + \dot{A}_G \dot{q}.
|
||||
|
||||
The centroidal dynamics :math:`\dot{h}_G` are then linked to external forces on the system by:
|
||||
|
||||
.. math:: \dot{h}_G = f_G^{net},
|
||||
|
||||
where :math:`f_G^{net}` is the net external wrench applied on the robot expressed at the CoM. This last term
|
||||
includes for instance the gravity force and ground reaction forces.
|
||||
|
||||
Warnings: this currently does not work with a fixed base.
|
||||
|
||||
Args:
|
||||
q (np.array[N], None): joint positions of size N, where N is the total number of free joints. If None, it
|
||||
will get the current joint positions (but note that this could lead to a decrease of performance).
|
||||
dq (np.array[N], None): joint velocities of size N, where N is the total number of free joints. If None, it
|
||||
will get the current joint velocities (but note that this could lead to a decrease of performance).
|
||||
inertia (np.array[6+N,6+N]): inertia matrix. If None, it will get the current inertia matrix
|
||||
(but note that this could lead to a decrease of performance if you have already computed it).
|
||||
|
||||
Returns:
|
||||
np.array[6, N+6]: the centroidal momentum matrix :math:`A_G`
|
||||
np.array[6]: the centroidal dynamics velocity-dependent bias vector :math:`\dot{A}_G \dot{q}`
|
||||
|
||||
Raises:
|
||||
RuntimeError: if the robot has not a floating base (i.e. it has a fixed base).
|
||||
|
||||
References:
|
||||
[1] "Improved computation of the humanoid centroidal dynamics and application for whole-body control",
|
||||
Wensing and Orin, 2016
|
||||
[2] "Motion Planning and Control of Dynamic Humanoid Locomotion" (PhD thesis: sec 2.1.5 and 3.1.3), Xin,
|
||||
2018
|
||||
"""
|
||||
# check if floating base
|
||||
if self.fixed_base:
|
||||
raise RuntimeError("You can not get the centroidal dynamics for a body with a fixed base; need to be a "
|
||||
"floating base.")
|
||||
|
||||
# get number of DoFs
|
||||
N, n = self.num_dofs, self.num_dofs - 6 # N=n+6
|
||||
|
||||
# check arguments
|
||||
if q is None:
|
||||
q = self.get_joint_positions() # shape = (N,)
|
||||
if dq is None:
|
||||
dq = self.get_joint_velocities() # shape = (N,)
|
||||
if inertia is None:
|
||||
inertia = self.get_mass_matrix(q) # shape = (N,N)
|
||||
H = inertia
|
||||
|
||||
# Coriolis term: C(q, dq) * dq
|
||||
C_dq = self.get_coriolis_torques(q=q, dq=dq) # shape = (N,)
|
||||
|
||||
# U_1
|
||||
U_1 = np.hstack((np.identity(6), np.zeros((6, n)))) # shape = (6,n+6)
|
||||
|
||||
# R_01(q_1)
|
||||
R_01 = get_matrix_from_quaternion(self.get_base_orientation()).T # shape = (3,3)
|
||||
|
||||
# Phi_1: this is a matrix transfer generalized velocity of floating base to spatial velocity defined in local
|
||||
# frame
|
||||
Phi_1 = np.vstack((np.hstack((np.zeros((3, 3)), np.identity(3))),
|
||||
np.hstack((R_01.T, np.zeros((3, 3)))))) # shape = (6,6)
|
||||
|
||||
# Psi_1
|
||||
Psi_1 = np.linalg.inv(Phi_1) # shape = (6,6)
|
||||
|
||||
# some other computations
|
||||
H11 = U_1.dot(H).dot(U_1.T) # shape = (6,6)
|
||||
I1C = Psi_1.T.dot(H11).dot(Psi_1) # shape = (6,6)
|
||||
M = I1C[6 - 1, 6 - 1]
|
||||
p1G = (1.0 / M) * np.array([I1C[3 - 1, 5 - 1], I1C[1 - 1, 6 - 1], I1C[2 - 1, 4 - 1]]) # shape = (3,)
|
||||
X_iG_T = np.vstack((np.hstack((R_01, R_01.dot(skew_matrix(p1G).T))),
|
||||
np.hstack((np.zeros((3, 3)), R_01)))) # shape = (6,6)
|
||||
|
||||
# compute centroidal momentum matrix and the dot product between the derivative of this centroidal momentum
|
||||
# matrix with the generalized velocities vector
|
||||
A_G = X_iG_T.dot(Psi_1.T).dot(U_1).dot(H) # shape = (6,n+6)
|
||||
A_Gd_dq = X_iG_T.dot(Psi_1.T).dot(U_1).dot(C_dq) # shape = (6,)
|
||||
|
||||
return A_G, A_Gd_dq
|
||||
|
||||
def get_centroidal_momentum(self, q=None, dq=None, inertia=None):
|
||||
r"""
|
||||
Return the centroidal momentum which consists of the net linear and angular momentum about the rigid-body's
|
||||
center of mass (CoM). This is thus the sum of all body spatial momenta computed with respect to the CoM,
|
||||
given by:
|
||||
|
||||
.. math:: h_G = A_G \dot{q},
|
||||
|
||||
where :math:`h_G \in \mathbb{R}^6` is the centroidal momentum, :math:`A_G \in \mathbb{R}^{6 \times (n+6)}` is
|
||||
the centroidal momentum matrix (CMM), and :math:`\dot{q}` are the system's generalized velocities. The CMM is
|
||||
related to the joint space inertia matrix (see code). This centroidal momentum collects the system linear and
|
||||
angular momentum together.
|
||||
|
||||
Args:
|
||||
q (np.array[N], None): joint positions of size N, where N is the total number of free joints. If None, it
|
||||
will get the current joint positions (but note that this could lead to a decrease of performance).
|
||||
dq (np.array[N], None): joint velocities of size N, where N is the total number of free joints. If None, it
|
||||
will get the current joint velocities (but note that this could lead to a decrease of performance).
|
||||
inertia (np.array[6+N,6+N]): inertia matrix. If None, it will get the current inertia matrix
|
||||
(but note that this could lead to a decrease of performance if you have already computed it).
|
||||
|
||||
Returns:
|
||||
np.array[6]: the centroidal momentum
|
||||
|
||||
Raises:
|
||||
RuntimeError: if the robot has not a floating base (i.e. it has a fixed base).
|
||||
|
||||
References:
|
||||
[1] "Improved computation of the humanoid centroidal dynamics and application for whole-body control",
|
||||
Wensing and Orin, 2016
|
||||
[2] "Motion Planning and Control of Dynamic Humanoid Locomotion" (PhD thesis: sec 2.1.5 and 3.1.3), Xin,
|
||||
2018
|
||||
"""
|
||||
if dq is None:
|
||||
dq = self.get_joint_velocities() # shape = (N,)
|
||||
A_G, A_Gd_dq = self.get_centroidal_dynamics(q, dq, inertia)
|
||||
return A_G.dot(dq)
|
||||
|
||||
@staticmethod
|
||||
def get_centroidal_momentum_singular_values(A_G):
|
||||
r"""
|
||||
Return the singular values of the centroidal momentum matrix.
|
||||
|
||||
Args:
|
||||
A_G (np.array[6, N+6]): centroidal momentum matrix
|
||||
|
||||
Returns:
|
||||
np.array[6]: singular values
|
||||
"""
|
||||
u, s, vh = np.linalg.svd(A_G, full_matrices=True)
|
||||
return s
|
||||
|
||||
@staticmethod
|
||||
def get_centroidal_momentum_orientation_and_scale(A_G):
|
||||
r"""
|
||||
Return the orientation and scale of the centroidal momentum matrix ellipsoid.
|
||||
|
||||
Args:
|
||||
A_G (np.array[6, N+6]): centroidal momentum matrix
|
||||
|
||||
Returns:
|
||||
np.array[4]: orientation (expressed as a quaternion [x,y,z,w])
|
||||
float: scale
|
||||
"""
|
||||
u, scale, vh = np.linalg.svd(A_G, full_matrices=True)
|
||||
quaternion = get_quaternion_from_matrix(u)
|
||||
return quaternion, scale
|
||||
|
||||
######################
|
||||
# Symbolic Equations #
|
||||
@@ -3474,6 +3739,16 @@ class Robot(ControllableBody):
|
||||
|
||||
@staticmethod
|
||||
def get_ellipsoid_orientation_and_scale(X):
|
||||
r"""
|
||||
Get ellipsoid's orientation and scale.
|
||||
|
||||
Args:
|
||||
X (np.array): 2D matrix
|
||||
|
||||
Returns:
|
||||
np.array[4]: orientation (expressed as a quaternion [x,y,z,w])
|
||||
float: scale
|
||||
"""
|
||||
# compute evecs and singular values
|
||||
_, S, V = np.linalg.svd(X)
|
||||
|
||||
|
||||
@@ -1948,7 +1948,7 @@ class Raisim(Simulator):
|
||||
pass
|
||||
|
||||
def calculate_jacobian(self, body_id, link_id, local_position, q, dq, des_ddq):
|
||||
"""
|
||||
r"""
|
||||
Return the full geometric Jacobian matrix :math:`J(q) = [J_{lin}(q), J_{ang}(q)]^T`, such that:
|
||||
|
||||
.. math:: v = [\dot{p}, \omega]^T = J(q) \dot{q}
|
||||
@@ -1974,7 +1974,7 @@ class Raisim(Simulator):
|
||||
pass
|
||||
|
||||
def calculate_mass_matrix(self, body_id, q):
|
||||
"""
|
||||
r"""
|
||||
Return the mass/inertia matrix :math:`H(q)`, which is used in the rigid-body equation of motion (EoM) in joint
|
||||
space given by (see [1]):
|
||||
|
||||
|
||||
@@ -1001,7 +1001,7 @@ class Simulator(object):
|
||||
|
||||
def set_joint_motor_control(self, body_id, joint_ids, control_mode=2, positions=None,
|
||||
velocities=None, forces=None, kp=None, kd=None, max_velocity=None):
|
||||
"""
|
||||
r"""
|
||||
Set the joint motor control.
|
||||
|
||||
In position control:
|
||||
@@ -2083,7 +2083,7 @@ class Simulator(object):
|
||||
pass
|
||||
|
||||
def calculate_jacobian(self, body_id, link_id, local_position, q, dq, des_ddq):
|
||||
"""
|
||||
r"""
|
||||
Return the full geometric Jacobian matrix :math:`J(q) = [J_{lin}(q), J_{ang}(q)]^T`, such that:
|
||||
|
||||
.. math:: v = [\dot{p}, \omega]^T = J(q) \dot{q}
|
||||
@@ -2109,7 +2109,7 @@ class Simulator(object):
|
||||
pass
|
||||
|
||||
def calculate_mass_matrix(self, body_id, q):
|
||||
"""
|
||||
r"""
|
||||
Return the mass/inertia matrix :math:`H(q)`, which is used in the rigid-body equation of motion (EoM) in joint
|
||||
space given by (see [1]):
|
||||
|
||||
@@ -2134,7 +2134,7 @@ class Simulator(object):
|
||||
def calculate_inverse_kinematics(self, body_id, link_id, position, orientation=None, lower_limits=None,
|
||||
upper_limits=None, joint_ranges=None, rest_poses=None, joint_dampings=None,
|
||||
solver=None, q_curr=None, max_iters=None, threshold=None):
|
||||
"""
|
||||
r"""
|
||||
Compute the FULL Inverse kinematics; it will return a position for all the actuated joints.
|
||||
|
||||
"You can compute the joint angles that makes the end-effector reach a given target position in Cartesian world
|
||||
|
||||
Reference in New Issue
Block a user