def test_frame_algo(self):
model = self.model
data = model.createData()
q = pin.neutral(model)
v = np.random.rand((model.nv))
frame_id = self.frame_idx
J1 = pin.computeFrameJacobian(model, data, q, frame_id)
J2 = pin.computeFrameJacobian(model, data, q, frame_id, pin.LOCAL)
self.assertApprox(J1, J2)
data2 = model.createData()
pin.computeJointJacobians(model, data2, q)
J3 = pin.getFrameJacobian(model, data2, frame_id, pin.LOCAL)
self.assertApprox(J1, J3)
dJ1 = pin.frameJacobianTimeVariation(model, data, q, v, frame_id,
pin.LOCAL)
data3 = model.createData()
pin.computeJointJacobiansTimeVariation(model, data3, q, v)
dJ2 = pin.getFrameJacobianTimeVariation(model, data3, frame_id,
pin.LOCAL)
self.assertApprox(dJ1, dJ2)
def invgeom(h = 0.2, lx = 0.1946, ly=0.16891, leg_dir = [+1,+1,-1,-1]):
#Inputs to be modified by the user
feet_position_ref = [np.array([lx, ly, 0.0191028]),np.array([lx, -ly, 0.0191028]),np.array([-lx, ly, 0.0191028]),np.array([-lx, -ly, 0.0191028])]
base_orientation_ref = pin.utils.rpyToMatrix(0,0,0)
com_ref = np.array([0,0,h])
FL_FOOT_ID = robot.model.getFrameId('FL_FOOT')
FR_FOOT_ID = robot.model.getFrameId('FR_FOOT')
HL_FOOT_ID = robot.model.getFrameId('HL_FOOT')
HR_FOOT_ID = robot.model.getFrameId('HR_FOOT')
BASE_ID = robot.model.getFrameId('base_link')
foot_ids = np.array([FL_FOOT_ID, FR_FOOT_ID, HL_FOOT_ID, HR_FOOT_ID])
q = np.array([ 0., 0.,0.235, 0. , 0. , 0. , 1. ,
0.1 , +0.8 * leg_dir[0] , -1.6 * leg_dir[0] ,
-0.1 , +0.8 * leg_dir[1], -1.6 * leg_dir[1] ,
0.1 , -0.8 * leg_dir[2] , +1.6 * leg_dir[2] ,
-0.1 , -0.8 * leg_dir[3] , +1.6 * leg_dir[3]])
dq = robot.v0.copy()
for i in range(100):
#Compute fwd kin
pin.forwardKinematics(rmodel,rdata,q,np.zeros(rmodel.nv),np.zeros(rmodel.nv))
pin.updateFramePlacements(rmodel,rdata)
### FEET
Jfeet = []
pfeet_err = []
for i_ee in range(4):
idx = int(foot_ids[i_ee])
pos = rdata.oMf[idx].translation
ref = feet_position_ref[i_ee]
Jfeet.append(pin.computeFrameJacobian(robot.model,robot.data,q,idx,pin.LOCAL_WORLD_ALIGNED)[:3])
pfeet_err.append(ref-pos)
### CoM
com = robot.com(q)
Jcom = robot.Jcom(q)
e_com = com_ref-com
### BASE ROTATION
idx = BASE_ID
rot = rdata.oMf[idx].rotation
rotref = base_orientation_ref
Jwbasis = pin.computeFrameJacobian(robot.model,robot.data,q,idx,pin.LOCAL_WORLD_ALIGNED)[3:]
e_basisrot = -rotref @ pin.log3(rotref.T@rot)
#All tasks
J = np.vstack(Jfeet+[Jcom,Jwbasis])
x_err = np.concatenate(pfeet_err+[e_com,e_basisrot])
residual = np.linalg.norm(x_err)
if residual<1e-5:
print (np.linalg.norm(x_err))
print ("iteration: {} residual: {}".format(i,residual))
return q
dq=np.linalg.pinv(J)@(x_err)
q=pin.integrate(robot.model,q,dq)
return q * np.nan
def compute_jacobian(self, q):
'''
computes the jacobian in the world frame
Math : J = R(World,Foot) * J_(Foot frame)
Selection of the required portion of the jacobian is also done here
'''
self.compute_forward_kinematics(q)
jac = pin.computeFrameJacobian(self.pin_robot.model,
self.pin_robot.data, q,
self.frame_end_idx)
jac = self.pin_robot.data.oMf[self.frame_end_idx].rotation.dot(
jac[0:3])
return jac
def compute_relative_velocity_between_frames(self, q, dq):
'''
computes the velocity of the end_frame with respect to a frame
whose origin aligns with the root frame but is oriented as the world frame
'''
# TODO: define relative vel with respect to frame oriented as the base frame but located at root frame
## will be a problem in case of a back flip with current implementation.
frame_config_root = pin.SE3(
self.pin_robot.data.oMf[self.frame_root_idx].rotation,
np.zeros((3, 1)))
frame_config_end = pin.SE3(
self.pin_robot.data.oMf[self.frame_end_idx].rotation,
np.zeros((3, 1)))
vel_root_in_world_frame = frame_config_root.action.dot(
pin.computeFrameJacobian(self.pin_robot.model, self.pin_robot.data,
q, self.frame_root_idx)).dot(dq)[0:3]
vel_end_in_world_frame = frame_config_end.action.dot(
pin.computeFrameJacobian(self.pin_robot.model, self.pin_robot.data,
q, self.frame_end_idx)).dot(dq)[0:3]
return np.subtract(vel_end_in_world_frame, vel_root_in_world_frame).T
def compute_jacobian(self, finger_id, q0):
"""
Compute the jacobian of a finger at configuration q0.
Args:
finger_id (int): an index specifying the end effector. (i.e. 0 for
the first finger, 1 for the second, ...)
q0 (np.array): The current joint positions.
Returns:
An np.array containing the jacobian.
"""
frame_id = self.tip_link_ids[finger_id]
return pinocchio.computeFrameJacobian(
self.robot_model,
self.data,
q0,
frame_id,
pinocchio.ReferenceFrame.LOCAL_WORLD_ALIGNED,
)
def id_joint_torques(self, q, dq, des_q, des_v, des_a, fff, cnt_array):
"""
This function computes the input torques with gains
Input:
q : joint positions
dq : joint velocity
des_q : desired joint positions
des_v : desired joint velocities
des_a : desired joint accelerations
fff : desired feed forward force
cnt_array
"""
assert len(q) == self.nq
self.J_arr = []
tau_id = self.compute_id_torques(des_q, des_v, des_a)
## creating QP matrices
N = int(len(self.eff_arr))
for j in range(N):
self.J_arr.append(pin.computeFrameJacobian(self.pinModel, self.pinData, des_q,\
self.pinModel.getFrameId(self.eff_arr[j]), pin.LOCAL_WORLD_ALIGNED).T)
tau_eff = np.zeros(self.nv)
for j in range(N):
if fff[(j * 3) + 2] > 0:
tau_eff += np.matmul(
self.J_arr[j],
np.hstack((fff[j * 3:(j + 1) * 3], np.zeros(3))))
tau = (tau_id - tau_eff)[6:]
tau_gain = -self.kp * (np.subtract(q[7:].T, des_q[7:].T)) - self.kd * (
np.subtract(dq[6:].T, des_v[6:].T))
return tau + tau_gain.T
def calcDiff(self, q):
J = pin.computeFrameJacobian(self.rmodel, self.rdata, q,
self.frameIndex)
r = self.residual(q)
Jlog = pin.Jlog6(self.deltaM)
return 2 * J.T @ Jlog.T @ r
def kinInv_3D(self, q, qdot, solo, feet_traj):
from numpy.linalg import pinv
K = self.getParameter('kinv_gain') # Convergence gain
# unactuated, [x, y, z] position of the base + [x, y, z, w] orientation of the base (stored as a quaternion)
# qu = q[:7]
# [v_x, v_y, v_z] linear velocity of the base and [w_x, w_y, w_z] angular velocity of the base along x, y, z axes
# of the world
# qu_dot = qdot[:6]
qa_dot_ref = np.zeros(
(12, 1)) # target angular velocities for the motors
if self.flag_q_ref:
self.q_ref = solo.q0.copy()
self.flag_q_ref = False
# Compute/update all the joints and frames
pin.forwardKinematics(solo.model, solo.data, self.q_ref)
pin.updateFramePlacements(solo.model, solo.data)
# Get the frame index of each foot
ID_FL = solo.model.getFrameId("FL_FOOT")
ID_FR = solo.model.getFrameId("FR_FOOT")
ID_HL = solo.model.getFrameId("HL_FOOT")
ID_HR = solo.model.getFrameId("HR_FOOT")
# Get the current position of the feet
xyz_FL = solo.data.oMf[ID_FL].translation
xyz_FR = solo.data.oMf[ID_FR].translation
xyz_HL = solo.data.oMf[ID_HL].translation
xyz_HR = solo.data.oMf[ID_HR].translation
# Desired foot trajectory
xyzdes_FL = feet_traj[0]
xyzdes_FR = feet_traj[1]
xyzdes_HL = feet_traj[2]
xyzdes_HR = feet_traj[3]
# Calculating the error
err_FL = (xyz_FL - xyzdes_FL)
err_FR = (xyz_FR - xyzdes_FR)
err_HL = (xyz_HL - xyzdes_HL)
err_HR = (xyz_HR - xyzdes_HR)
# Computing the local Jacobian into the global frame
oR_FL = solo.data.oMf[ID_FL].rotation
oR_FR = solo.data.oMf[ID_FR].rotation
oR_HL = solo.data.oMf[ID_HL].rotation
oR_HR = solo.data.oMf[ID_HR].rotation
# Getting the different Jacobians
fJ_FL3 = pin.computeFrameJacobian(
solo.model, solo.data, self.q_ref,
ID_FL)[:3, -12:] # Take only the translation terms
oJ_FL3 = oR_FL.dot(
fJ_FL3) # Transformation from local frame to world frame
oJ_FLxyz = oJ_FL3[0:3, -12:] # Take the x,y & z components
fJ_FR3 = pin.computeFrameJacobian(solo.model, solo.data, self.q_ref,
ID_FR)[:3, -12:]
oJ_FR3 = oR_FR.dot(fJ_FR3)
oJ_FRxyz = oJ_FR3[0:3, -12:]
fJ_HL3 = pin.computeFrameJacobian(solo.model, solo.data, self.q_ref,
ID_HL)[:3, -12:]
oJ_HL3 = oR_HL.dot(fJ_HL3)
oJ_HLxyz = oJ_HL3[0:3, -12:]
fJ_HR3 = pin.computeFrameJacobian(solo.model, solo.data, self.q_ref,
ID_HR)[:3, -12:]
oJ_HR3 = oR_HR.dot(fJ_HR3)
oJ_HRxyz = oJ_HR3[0:3, -12:]
# Displacement error
nu = np.hstack([err_FL, err_FR, err_HL, err_HR])
nu = np.matrix(nu)
nu = np.transpose(nu)
# Making a single x&y&z-rows Jacobian vector
J = np.vstack([oJ_FLxyz, oJ_FRxyz, oJ_HLxyz, oJ_HRxyz])
# Computing the velocity
qa_dot_ref = -K * pinv(J) * nu
q_dot_ref = np.concatenate((np.zeros([6, 1]), qa_dot_ref))
# Computing the updated configuration
self.q_ref = pin.integrate(solo.model, self.q_ref,
q_dot_ref * self.getParameter('dt'))
qa_ref = self.q_ref[7:].reshape(12, 1)
# Return configuration of the robot
q_dot_ref = np.squeeze(np.array(q_dot_ref))
err = np.linalg.norm(nu)
return self.q_ref, q_dot_ref, err
DT = 1e-2 # Integration step.
q0 = np.matrix([[
0., 0., 0., 1., 0.18, 1.37, -0.24, -0.98, 0.98, 0., 0., 0., 0., -0.13, 0.,
0., 0., 0.
]]).T
q = q0.copy()
herr = [] # Log the value of the error between tool and goal.
# Loop on an inverse kinematics for 200 iterations.
for i in range(200): # Integrate over 2 second of robot life
pio.framesForwardKinematics(robot.model, robot.data,
q) # Compute frame placements
oMtool = robot.data.oMf[
IDX_TOOL] # Placement from world frame o to frame f oMtool
oRtool = oMtool.rotation # Rotation from world axes to tool axes oRtool
tool_Jtool = pio.computeFrameJacobian(
robot.model, robot.data, q, IDX_TOOL) # 6D jacobian in local frame
o_Jtool3 = oRtool * tool_Jtool[:3, :] # 3D jacobian in world frame
o_TG = oMtool.translation - oMgoal.translation # vector from tool to goal, in world frame
vq = -pinv(o_Jtool3) * o_TG
q = pio.integrate(robot.model, q, vq * DT)
robot.display(q)
time.sleep(1e-3)
herr.append(o_TG)
q = q0.copy()
herr = []
for i in range(1000): # Integrate over 2 second of robot life
pio.framesForwardKinematics(robot.model, robot.data,
def compute(self, q, dq):
### FEET
Jfeet = []
afeet = []
pfeet_err = []
vfeet_ref = []
pin.forwardKinematics(self.rmodel, self.rdata, q, dq,
np.zeros(self.rmodel.nv))
pin.updateFramePlacements(self.rmodel, self.rdata)
for i_ee in range(4):
idx = int(self.foot_ids[i_ee])
pos = self.rdata.oMf[idx].translation
nu = pin.getFrameVelocity(self.rmodel, self.rdata, idx,
pin.LOCAL_WORLD_ALIGNED)
ref = self.feet_position_ref[i_ee]
vref = self.feet_velocity_ref[i_ee]
aref = self.feet_acceleration_ref[i_ee]
J1 = pin.computeFrameJacobian(self.robot.model, self.robot.data, q,
idx, pin.LOCAL_WORLD_ALIGNED)[:3]
e1 = ref - pos
acc1 = -self.Kp_flyingfeet * (pos - ref) - self.Kd_flyingfeet * (
nu.linear - vref) + aref
if self.flag_in_contact[i_ee]:
acc1 *= 0 # In contact = no feedback
drift1 = np.zeros(3)
drift1 += pin.getFrameAcceleration(self.rmodel, self.rdata, idx,
pin.LOCAL_WORLD_ALIGNED).linear
drift1 += self.cross3(nu.angular, nu.linear)
acc1 -= drift1
Jfeet.append(J1)
afeet.append(acc1)
pfeet_err.append(e1)
vfeet_ref.append(vref)
### BASE POSITION
idx = self.BASE_ID
pos = self.rdata.oMf[idx].translation
nu = pin.getFrameVelocity(self.rmodel, self.rdata, idx,
pin.LOCAL_WORLD_ALIGNED)
ref = self.base_position_ref
Jbasis = pin.computeFrameJacobian(self.robot.model, self.robot.data, q,
idx, pin.LOCAL_WORLD_ALIGNED)[:3]
e_basispos = ref - pos
accbasis = -self.Kp_base_position * (
pos - ref) - self.Kd_base_position * nu.linear
drift = np.zeros(3)
drift += pin.getFrameAcceleration(self.rmodel, self.rdata, idx,
pin.LOCAL_WORLD_ALIGNED).linear
drift += self.cross3(nu.angular, nu.linear)
accbasis -= drift
### BASE ROTATION
idx = self.BASE_ID
rot = self.rdata.oMf[idx].rotation
nu = pin.getFrameVelocity(self.rmodel, self.rdata, idx,
pin.LOCAL_WORLD_ALIGNED)
rotref = self.base_orientation_ref
Jwbasis = pin.computeFrameJacobian(self.robot.model, self.robot.data,
q, idx, pin.LOCAL_WORLD_ALIGNED)[3:]
e_basisrot = -rotref @ pin.log3(rotref.T @ rot)
accwbasis = -self.Kp_base_orientation * rotref @ pin.log3(
rotref.T @ rot) - self.Kd_base_orientation * nu.angular
drift = np.zeros(3)
drift += pin.getFrameAcceleration(self.rmodel, self.rdata, idx,
pin.LOCAL_WORLD_ALIGNED).angular
accwbasis -= drift
J = np.vstack(Jfeet + [Jbasis, Jwbasis])
acc = np.concatenate(afeet + [accbasis, accwbasis])
x_err = np.concatenate(pfeet_err + [e_basispos, e_basisrot])
dx_ref = np.concatenate(
vfeet_ref +
[self.base_linearvelocity_ref, self.base_angularvelocity_ref])
invJ = self.dinv(J) #or np.linalg.inv(J) since full rank
ddq = invJ @ acc
self.q_cmd = pin.integrate(self.robot.model, q, invJ @ x_err)
self.dq_cmd = invJ @ dx_ref
return ddq
def calcDiff6d(self, q):
J = pin.computeFrameJacobian(robot.model, robot.data, q,
self.frameIndex)
r = self.residual6d(q)
Jlog = pin.Jlog6(self.deltaM)
return 2 * J.T @ Jlog.T @ r
def c_walking_IK(q, qdot, dt, solo, t_simu):
qu = q[:
7] # unactuated, [x, y, z] position of the base + [x, y, z, w] orientation of the base (stored as a quaternion)
qa = q[7:] # actuated, [q1, q2, ..., q8] angular position of the 8 motors
qu_dot = qdot[:
6] # [v_x, v_y, v_z] linear velocity of the base and [w_x, w_y, w_z] angular velocity of the base along x, y, z axes of the world
qa_dot = qdot[6:] # angular velocity of the 8 motors
qa_ref = np.zeros((8, 1)) # target angular positions for the motors
qa_dot_ref = np.zeros((8, 1)) # target angular velocities for the motors
###############################################
# Insert code here to set qa_ref and qa_dot_ref
from numpy.linalg import pinv
global q_ref, flag_q_ref, T, dx, dz
if flag_q_ref:
q_ref = solo.q0.copy()
flag_q_ref = False
# Initialization of the variables
K = 100. #Convergence gain
T = 0.3 #period of the foot trajectory
xF0 = 0.19 #initial position of the front feet
xH0 = -0.19 #initial position of the hind feet
z0 = 0.0 #initial altitude of each foot
dx = 0.05 #displacement amplitude by x
dz = 0.1 #displacement amplitude by z
# Get the frame index of each foot
ID_FL = solo.model.getFrameId("FL_FOOT")
ID_FR = solo.model.getFrameId("FR_FOOT")
ID_HL = solo.model.getFrameId("HL_FOOT")
ID_HR = solo.model.getFrameId("HR_FOOT")
# function defining the feet's trajectory
def ftraj(t, x0, z0): #arguments : time, initial position x and z
global T, dx, dz
x = []
z = []
if t >= T:
t %= T
x.append(x0 - dx * np.cos(2 * np.pi * t / T))
if t <= T / 2.:
z.append(z0 + dz * np.sin(2 * np.pi * t / T))
else:
z.append(0)
return np.matrix([x, z])
# Compute/update all the joints and frames
pin.forwardKinematics(solo.model, solo.data, q_ref)
pin.updateFramePlacements(solo.model, solo.data)
# Get the current height (on axis z) and the x-coordinate of the front left foot
xz_FL = solo.data.oMf[ID_FL].translation[0::2]
xz_FR = solo.data.oMf[ID_FR].translation[0::2]
xz_HL = solo.data.oMf[ID_HL].translation[0::2]
xz_HR = solo.data.oMf[ID_HR].translation[0::2]
# Desired foot trajectory
xzdes_FL = ftraj(t_simu, xF0, z0)
xzdes_HR = ftraj(t_simu, xH0, z0)
xzdes_FR = ftraj(t_simu + T / 2, xF0, z0)
xzdes_HL = ftraj(t_simu + T / 2, xH0, z0)
# Calculating the error
err_FL = xz_FL - xzdes_FL
err_FR = xz_FR - xzdes_FR
err_HL = xz_HL - xzdes_HL
err_HR = xz_HR - xzdes_HR
# Computing the local Jacobian into the global frame
oR_FL = solo.data.oMf[ID_FL].rotation
oR_FR = solo.data.oMf[ID_FR].rotation
oR_HL = solo.data.oMf[ID_HL].rotation
oR_HR = solo.data.oMf[ID_HR].rotation
# Getting the different Jacobians
fJ_FL3 = pin.computeFrameJacobian(
solo.model, solo.data, q_ref,
ID_FL)[:3, -8:] #Take only the translation terms
oJ_FL3 = oR_FL * fJ_FL3 #Transformation from local frame to world frame
oJ_FLxz = oJ_FL3[0::2, -8:] #Take the x and z components
fJ_FR3 = pin.computeFrameJacobian(solo.model, solo.data, q_ref, ID_FR)[:3,
-8:]
oJ_FR3 = oR_FR * fJ_FR3
oJ_FRxz = oJ_FR3[0::2, -8:]
fJ_HL3 = pin.computeFrameJacobian(solo.model, solo.data, q_ref, ID_HL)[:3,
-8:]
oJ_HL3 = oR_HL * fJ_HL3
oJ_HLxz = oJ_HL3[0::2, -8:]
fJ_HR3 = pin.computeFrameJacobian(solo.model, solo.data, q_ref, ID_HR)[:3,
-8:]
oJ_HR3 = oR_HR * fJ_HR3
oJ_HRxz = oJ_HR3[0::2, -8:]
# Displacement error
nu = np.vstack([err_FL, err_FR, err_HL, err_HR])
# Making a single x&z-rows Jacobian vector
J = np.vstack([oJ_FLxz, oJ_FRxz, oJ_HLxz, oJ_HRxz])
# Computing the velocity
qa_dot_ref = -K * pinv(J) * nu
q_dot_ref = np.concatenate((np.zeros([6, 1]), qa_dot_ref))
# Computing the updated configuration
q_ref = pin.integrate(solo.model, q_ref, q_dot_ref * dt)
qa_ref = q_ref[7:]
# DONT FORGET TO RUN GEPETTO-GUI BEFORE RUNNING THIS PROGRAMM #
#solo.display(q) # display the robot in the viewer Gepetto-GUI given its configuration q
q_list.append(qa_ref)
# End of the control code
###############################################
# Parameters for the PD controller
Kp = 10.
Kd = 0.3
torque_sat = 3 # torque saturation in N.m
torques_ref = np.zeros((8, 1)) # feedforward torques
# Call the PD controller
torques = PD(qa_ref, qa_dot_ref, qa, qa_dot, dt, Kp, Kd, torque_sat,
torques_ref)
# torques must be a numpy array of shape (8, 1) containing the torques applied to the 8 motors
return torques, q_list
def c_walking_IK_bezier(q, qdot, dt, solo, t_simu):
qu = q[:
7] # unactuated, [x, y, z] position of the base + [x, y, z, w] orientation of the base (stored as a quaternion)
qa = q[7:] # actuated, [q1, q2, ..., q8] angular position of the 8 motors
qu_dot = qdot[:
6] # [v_x, v_y, v_z] linear velocity of the base and [w_x, w_y, w_z] angular velocity of the base along x, y, z axes of the world
qa_dot = qdot[6:] # angular velocity of the 8 motors
qa_ref = np.zeros((8, 1)) # target angular positions for the motors
qa_dot_ref = np.zeros((8, 1)) # target angular velocities for the motors
###############################################
# Insert code here to set qa_ref and qa_dot_ref
global q_ref, flag_q_ref, T
if flag_q_ref:
q_ref = solo.q0.copy()
flag_q_ref = False
# Get the frame index of each foot
ID_FL = solo.model.getFrameId("FL_FOOT")
ID_FR = solo.model.getFrameId("FR_FOOT")
ID_HL = solo.model.getFrameId("HL_FOOT")
ID_HR = solo.model.getFrameId("HR_FOOT")
# Compute/update all the joints and frames
pin.forwardKinematics(solo.model, solo.data, q_ref)
pin.updateFramePlacements(solo.model, solo.data)
# Get the current height (on axis z) and the x-coordinate of the front left foot
xz_FL = solo.data.oMf[ID_FL].translation[0::2]
xz_FR = solo.data.oMf[ID_FR].translation[0::2]
xz_HL = solo.data.oMf[ID_HL].translation[0::2]
xz_HR = solo.data.oMf[ID_HR].translation[0::2]
# Desired foot trajectory
xzdes_FL = ftraj_Front(t_simu)
xzdes_HR = ftraj_Hind(t_simu)
xzdes_FR = ftraj_Front(t_simu + T / 2)
xzdes_HL = ftraj_Hind(t_simu + T / 2)
# Calculating the error
err_FL = xz_FL - xzdes_FL
err_FR = xz_FR - xzdes_FR
err_HL = xz_HL - xzdes_HL
err_HR = xz_HR - xzdes_HR
# Computing the local Jacobian into the global frame
oR_FL = solo.data.oMf[ID_FL].rotation
oR_FR = solo.data.oMf[ID_FR].rotation
oR_HL = solo.data.oMf[ID_HL].rotation
oR_HR = solo.data.oMf[ID_HR].rotation
# Getting the different Jacobians
fJ_FL3 = pin.computeFrameJacobian(
solo.model, solo.data, q_ref,
ID_FL)[:3, -8:] #Take only the translation terms
oJ_FL3 = oR_FL * fJ_FL3 #Transformation from local frame to world frame
oJ_FLxz = oJ_FL3[0::2, -8:] #Take the x and z components
fJ_FR3 = pin.computeFrameJacobian(solo.model, solo.data, q_ref, ID_FR)[:3,
-8:]
oJ_FR3 = oR_FR * fJ_FR3
oJ_FRxz = oJ_FR3[0::2, -8:]
fJ_HL3 = pin.computeFrameJacobian(solo.model, solo.data, q_ref, ID_HL)[:3,
-8:]
oJ_HL3 = oR_HL * fJ_HL3
oJ_HLxz = oJ_HL3[0::2, -8:]
fJ_HR3 = pin.computeFrameJacobian(solo.model, solo.data, q_ref, ID_HR)[:3,
-8:]
oJ_HR3 = oR_HR * fJ_HR3
oJ_HRxz = oJ_HR3[0::2, -8:]
# Displacement error
nu = np.vstack([err_FL, err_FR, err_HL, err_HR])
# Making a single x&z-rows Jacobian vector
J = np.vstack([oJ_FLxz, oJ_FRxz, oJ_HLxz, oJ_HRxz])
# Computing the velocity
qa_dot_ref = -K * pinv(J) * nu
q_dot_ref = np.concatenate((np.zeros([6, 1]), qa_dot_ref))
# Computing the updated configuration
q_ref = pin.integrate(solo.model, q_ref, q_dot_ref * dt)
qa_ref = q_ref[7:]
# DONT FORGET TO RUN GEPETTO-GUI BEFORE RUNNING THIS PROGRAMM #
solo.display(
q
) # display the robot in the viewer Gepetto-GUI given its configuration q
# End of the control code
###############################################
# Parameters for the PD controller
Kp = 10.
Kd = 0.3
torque_sat = 2.5 # torque saturation in N.m
torques_ref = np.zeros((8, 1)) # feedforward torques
# Call the PD controller
torques = PD(qa_ref, qa_dot_ref, qa, qa_dot, dt, Kp, Kd, torque_sat,
torques_ref)
# torques must be a numpy array of shape (8, 1) containing the torques applied to the 8 motors
return torques, qa_ref, qa_dot_ref
DT = 1e-2 # Integration step.
q0 = np.matrix([[
0., 0., 0., 1., 0.18, 1.37, -0.24, -0.98, 0.98, 0., 0., 0., 0., -0.13, 0.,
0., 0., 0.
]]).T
q = q0.copy()
herr = [] # Log the value of the error between gaze and ball.
# Loop on an inverse kinematics for 200 iterations.
for i in range(200): # Integrate over 2 second of robot life
pio.framesForwardKinematics(robot.model, robot.data,
q) # Compute frame placements
oMgaze = robot.data.oMf[
IDX_GAZE] # Placement from world frame o to frame f oMgaze
oRgaze = oMgaze.rotation # Rotation from world axes to gaze axes oRgaze
gaze_Jgaze = pio.computeFrameJacobian(
robot.model, robot.data, q, IDX_GAZE) # 6D jacobian in local frame
o_Jgaze3 = oRgaze * gaze_Jgaze[:3, :] # 3D jacobian in world frame
o_GazeBall = oMgaze.translation - ball # vector from gaze to ball, in world frame
vq = -pinv(o_Jgaze3) * o_GazeBall
q = pio.integrate(robot.model, q, vq * DT)
robot.display(q)
time.sleep(1e-3)
herr.append(o_GazeBall)
q = q0.copy()
herr = [] # Log the value of the error between tool and goal.
herr2 = [] # Log the value of the error between gaze and ball.
# Loop on an inverse kinematics for 200 iterations.
