# load robot
robot = Centauro(sim, fixed_base=True)
# desired Velocity Manipulability for CoM
desired_velocity_manip = np.array([[0.01, 0.0, 0.0], [0.0, 0.04, 0.0],
[0.0, 0.0, 0.005]])
# proportional gain
Km = 200 * np.eye(6)
else:
raise NotImplementedError("The given robot has not been implemented")
# Initial conditions for visualization
# Display initial and desired manipulability ellipsoid
q0 = robot.get_joint_positions()
Jcom = robot.get_center_of_mass_jacobian(q0)
velocity_manip = robot.compute_velocity_manipulability_ellipsoid(Jcom)
robot.draw_velocity_manipulability_ellipsoid(link_id=-1,
JJT=10 * desired_velocity_manip,
color=(0.1, 0.75, 0.1, 0.6))
ellipsoid_id = robot.draw_velocity_manipulability_ellipsoid(
link_id=-1, JJT=10 * velocity_manip[0:3, 0:3], color=(0.75, 0.1, 0.1, 0.6))
# Run simulator
for i in count():
robot.compute_and_draw_com_position(radius=0.03)
print("CoM: {}".format(robot.get_center_of_mass_position()))
# get current joint position
qt = robot.get_joint_positions()
w = 0.01
r = 0.2
for t in count():
# move sphere
sphere.position = np.array([
0.5, r * np.cos(w * t + np.pi / 2),
(1. - r) + r * np.sin(w * t + np.pi / 2)
])
# get current end-effector position and velocity in the task/operational space
x = robot.get_link_world_positions(link_id)
dx = robot.get_link_world_linear_velocities(link_id)
# Get joint positions
q = robot.get_joint_positions()
# Get linear jacobian
if robot.has_floating_base():
J = robot.get_linear_jacobian(link_id, q=q)[:, qIdx + 6]
else:
J = robot.get_linear_jacobian(link_id, q=q)[:, qIdx]
# Pseudo-inverse: \hat{J} = J^T (JJ^T + k^2 I)^{-1}
Jp = robot.get_damped_least_squares_inverse(J, damping)
# evaluate damped-least-squares IK
dq = Jp.dot(kp * (sphere.position - x) - kd * dx)
# set joint positions
q = q[qIdx] + dq * dt
# OPTION 2: using Jacobian and manual damped-least-squares IK ###
elif solver_flag == 1:
kp = 50 # 5 if velocity control, 50 if position control
kd = 0 # 2*np.sqrt(kp)
for _ in count():
# get current position in the task/operational space
# x = robot.get_link_positions(link_id, wrt_link_id)
# dx = robot.get_link_linear_velocities(link_id, wrt_link_id)
x = robot.get_link_world_positions(link_id)
dx = robot.get_link_world_linear_velocities(link_id)
print("(xd - x) = {}".format(xd - x))
# Get joint configuration
q = robot.get_joint_positions()
# Get linear jacobian
if robot.has_floating_base():
J = robot.get_linear_jacobian(link_id, q=q)[:, qIdx + 6]
else:
J = robot.get_linear_jacobian(link_id, q=q)[:, qIdx]
# Pseudo-inverse
# Jp = robot.get_pinv_jacobian(J)
# Jp = J.T.dot(np.linalg.inv(J.dot(J.T) + damping*np.identity(3))) # this also works
Jp = robot.get_damped_least_squares_inverse(J, damping)
# evaluate damped-least-squares IK
dq = Jp.dot(kp * (xd - x) - kd * dx)