def compute(self):
frame_id = self.robot.model.getFrameId('lh_foot')
print pin.frameJacobian(self.robot.model, self.robot.data, frame_id, self.robot.q0)
print '---'
print pin.computeJacobians(self.robot.model, self.robot.data, self.robot.q0)
print '---'
print pin.se3.jacobian(self.robot.model, self.robot.data, self.robot.q0, frame_id, False, True) #False == world
def updateJacobiansAndState(self, kin_state, dt):
jnt_names = ["HFE", "KFE", "END"]
eff_names = ["BR", "BL", "FR", "FL"]
self.robot.q = np.transpose(
np.matrix(kin_state.robot_posture.generalized_joint_positions()))
self.robot.dq = np.transpose(
np.matrix(kin_state.robot_velocity.generalized_joint_velocities))
self.robot.ddq = np.transpose(
np.matrix(
kin_state.robot_acceleration.generalized_joint_accelerations))
self.robot.centroidalMomentumVariation(self.robot.q, self.robot.dq,
self.robot.ddq)
'Update objective function'
kin_state.com = self.robot.com(self.robot.q)
kin_state.lmom = self.robot.data.hg.vector[:3]
kin_state.amom = self.robot.data.hg.vector[3:]
self.centroidal_momentum_matrix = self.robot.data.Ag
self.centroidal_momentum_matrix_variation = self.robot.data.dAg
self.center_of_mass_jacobian = self.robot.Jcom(self.robot.q)
for eff_id in range(0, len(eff_names)):
nameToIndexMap = self.robot.model.getFrameId(eff_names[eff_id] +
"_" + jnt_names[-1])
self.endeffector_jacobians[eff_id] = pin.frameJacobian(
self.robot.model, self.robot.data, self.robot.q,
nameToIndexMap, pin.ReferenceFrame.LOCAL)[0:3, :]
kin_state.endeffector_positions[eff_id] = np.squeeze(
np.array(self.robot.data.oMf[nameToIndexMap].translation))
'Update constraints'
num_constraints = len(eff_names) * len(jnt_names)
G = np.zeros((num_constraints, self.robot.nv))
h = np.zeros((num_constraints))
for eff_id in range(0, len(eff_names)):
for jnt_id in range(0, len(jnt_names)):
nameToIndexMap = self.robot.model.getFrameId(
eff_names[eff_id] + "_" + jnt_names[jnt_id])
G[eff_id * len(jnt_names) + jnt_id, :] = np.squeeze(
np.asarray(
pin.frameJacobian(self.robot.model, self.robot.data,
self.robot.q, nameToIndexMap,
pin.ReferenceFrame.LOCAL)[2, :]))
h[eff_id * len(jnt_names) + jnt_id] = self.robot.data.oMf[
nameToIndexMap].translation[-1][0, 0]
self.constraintsMatrix = -G * dt
self.constraintsVector = h - self.z_floor
return kin_state
def footInverseKinematicsFixedBase(self, foot_pos_des, frame_name):
frame_id = self.model.getFrameId(frame_name)
blockIdx = self.getBlockIndex(frame_name)
anymal_q0 = np.vstack([-0.1, 0.7, -1., -0.1, -0.7, 1., 0.1, 0.7, -1., 0.1, -0.7, 1.])
q = anymal_q0
eps = 0.005
IT_MAX = 200
DT = 1e-1
err = np.zeros((3, 1))
e = np.zeros((3, 1))
i = 0
while True:
pinocchio.forwardKinematics(self.model, self.data, q)
pinocchio.framesForwardKinematics(self.model, self.data, q)
foot_pos = self.data.oMf[frame_id].translation
# err = np.hstack([err, (foot_pos - foot_pos_des)])
# e = err[:,-1]
# print foot_pos_des[0], foot_pos[[0]], foot_pos[[0]] - foot_pos_des[0]
# e = foot_pos - foot_pos_des
e[0] = foot_pos[[0]] - foot_pos_des[0]
e[1] = foot_pos[[1]] - foot_pos_des[1]
e[2] = foot_pos[[2]] - foot_pos_des[2]
J = pinocchio.frameJacobian(self.model, self.data, q, frame_id)
J_lin = J[:3, :]
if np.linalg.norm(e) < eps:
# print("IK Convergence achieved!")
IKsuccess = True
break
if i >= IT_MAX:
print(
"\n Warning: the iterative algorithm has not reached convergence to the desired precision. Error is: ",
np.linalg.norm(e))
IKsuccess = False
break
# print J_lin
v = - np.linalg.pinv(J_lin) * e
q = pinocchio.integrate(self.model, q, v * DT)
i += 1
# print i
q_leg = q[blockIdx:blockIdx + 3]
J_leg = J_lin[:, blockIdx:blockIdx + 3]
return q_leg, J_leg, err, IKsuccess
def frameJacobian(self, q, index, update_geometry=True, local_frame=True):
return se3.frameJacobian(self.model, self.data, q, index, local_frame,
update_geometry)
# Computing the error in the world frame
err_FL = np.concatenate((np.zeros([2, 1]), hFL))
err_FR = np.concatenate((np.zeros([2, 1]), hFR))
err_HL = np.concatenate((np.zeros([2, 1]), hHL))
err_HR = np.concatenate((np.zeros([2, 1]), hHR))
# Computing the error in the local 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.frameJacobian(solo.model, solo.data, q,
ID_FL)[:3,
-8:] #Taking only the translation terms
oJ_FL3 = oR_FL * fJ_FL3 #Transformation from local frame to world frame
oJ_FLz = oJ_FL3[2, -8:] #Taking the z_component
fJ_FR3 = pin.frameJacobian(solo.model, solo.data, q, ID_FR)[:3, -8:]
oJ_FR3 = oR_FR * fJ_FR3
oJ_FRz = oJ_FR3[2, -8:]
fJ_HL3 = pin.frameJacobian(solo.model, solo.data, q, ID_HL)[:3, -8:]
oJ_HL3 = oR_HL * fJ_HL3
oJ_HLz = oJ_HL3[2, -8:]
fJ_HR3 = pin.frameJacobian(solo.model, solo.data, q, ID_HR)[:3, -8:]
oJ_HR3 = oR_HR * fJ_HR3
oJ_HRz = oJ_HR3[2, -8:]
def place(name, M):
solo.viewer.gui.applyConfiguration(name, se3ToXYZQUAT(M))
solo.viewer.gui.refresh()
def Rquat(x, y, z, w):
q = pin.Quaternion(x, y, z, w)
q.normalize()
return q.matrix()
### Moving the basis along x_axis
embed()
ID_BASIS = solo.model.getFrameId('base_link')
q_basis = sq
dt = 1e-2
for j in range(200):
pin.forwardKinematics(solo.model, solo.data, q_basis)
pin.updateFramePlacements(solo.model, solo.data)
Mbasis = solo.data.oMf[ID_BASIS]
error = Mbasis.translation[0] - 1
J_basis = pin.frameJacobian(solo.model, solo.data, q_basis, ID_BASIS)[0, :]
nu_basis = error
vq_basis = pinv(J_basis) * nu_basis
q_basis = pin.integrate(solo.model, q_basis, vq_basis * dt)
solo.display(q_basis)
sleep(dt)
embed()
print 'Elapsed time = %.2f (simu time=%.2f)' % (time.time()-t0,simu.dt*niter)
return q,v
class Stab:
def __init__(self):
pass
q = robot.q0.copy()
v = zero(NV)
v = rand(NV)
#se3.crba(robot.model,robot.data,q)
se3.computeAllTerms(robot.model,robot.data,q,v)
M = robot.data.M
b = robot.data.nle
Jr = se3.frameJacobian(robot.model,robot.data,q,RF,True,False)
Jl = se3.frameJacobian(robot.model,robot.data,q,LF,True,False)
ark = robot.data.a[RK]
ar = robot.model.frames[RF].placement.inverse()*ark
alk = robot.data.a[LK]
al = robot.model.frames[LF].placement.inverse()*ark
Jcom = robot.data.Jcom
se3.centerOfMass(robot.model,robot.data,q,v,zero(NV))
acom = robot.data.acom[0]
def callback_torques():
global v_prev, solo, stop, q
jointStates = p.getJointStates(robotId,
revoluteJointIndices) # State of all joints
baseState = p.getBasePositionAndOrientation(robotId)
baseVel = p.getBaseVelocity(robotId)
# Info about contact points with the ground
contactPoints_FL = p.getContactPoints(robotId, planeId,
linkIndexA=2) # Front left foot
contactPoints_FR = p.getContactPoints(robotId, planeId,
linkIndexA=5) # Front right foot
contactPoints_HL = p.getContactPoints(robotId, planeId,
linkIndexA=8) # Hind left foot
contactPoints_HR = p.getContactPoints(robotId, planeId,
linkIndexA=11) # Hind right foot
# Sort contacts points to get only one contact per foot
contactPoints = []
contactPoints.append(getContactPoint(contactPoints_FL))
contactPoints.append(getContactPoint(contactPoints_FR))
contactPoints.append(getContactPoint(contactPoints_HL))
contactPoints.append(getContactPoint(contactPoints_HR))
# Joint vector for Pinocchio
q = np.vstack(
(np.array([baseState[0]]).transpose(), np.array([baseState[1]
]).transpose(),
np.array([[
jointStates[i_joint][0] for i_joint in range(len(jointStates))
]]).transpose()))
v = np.vstack(
(np.array([baseVel[0]]).transpose(), np.array([baseVel[1]
]).transpose(),
np.array([[
jointStates[i_joint][1] for i_joint in range(len(jointStates))
]]).transpose()))
v_dot = (v - v_prev) / h
v_prev = v.copy()
# Update display in Gepetto-gui
solo.display(q)
########################################################################
### Space mouse configuration
# return the next event if there is any
event = sp.poll()
# if event signals the release of the first button
# exit condition : the button 0 is pressed and released
if type(event) is sp.ButtonEvent \
and event.button == 0 and event.pressed == 0:
# set exit condition
stop = True
# matching the mouse signals with the position of Solo's basis
if type(event) is sp.MotionEvent:
Vroll = -event.rx / 100.0 #velocity related to the roll axis
Vyaw = event.ry / 100.0 #velocity related to the yaw axis
Vpitch = -event.rz / 100.0 #velocity related to the pitch axis
rpy[0] += Vroll * dt #roll
rpy[1] += Vpitch * dt #pitch
rpy[2] += Vyaw * dt #yaw
# adding saturation to prevent unlikely configurations
for i in range(2):
if rpy[i] > 0.175 or rpy[i] < -0.175:
rpy[i] = np.sign(rpy[i]) * 0.175
# convert rpy to quaternion
quatMat = pin.utils.rpyToMatrix(np.matrix(rpy).T)
# add the modified component to the quaternion
q[3] = Quaternion(quatMat)[0]
q[4] = Quaternion(quatMat)[1]
q[5] = Quaternion(quatMat)[2]
q[6] = Quaternion(quatMat)[3]
### Stack of Task : feet on the ground and posture
# compute/update all joints and frames
pin.forwardKinematics(solo.model, solo.data, q)
pin.updateFramePlacements(solo.model, solo.data)
# Getting the current height (on axis z) of each foot
hFL = solo.data.oMf[ID_FL].translation[2]
hFR = solo.data.oMf[ID_FR].translation[2]
hHL = solo.data.oMf[ID_HL].translation[2]
hHR = solo.data.oMf[ID_HR].translation[2]
# Computing the error in the world frame
err_FL = np.concatenate((np.zeros([2, 1]), hFL))
err_FR = np.concatenate((np.zeros([2, 1]), hFR))
err_HL = np.concatenate((np.zeros([2, 1]), hHL))
err_HR = np.concatenate((np.zeros([2, 1]), hHR))
# Error of posture
err_post = q - qdes
# Computing the error in the local 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.frameJacobian(solo.model, solo.data, q,
ID_FL)[:3,
-8:] #Take only the translation terms
oJ_FL3 = oR_FL * fJ_FL3 #Transformation from local frame to world frame
oJ_FLz = oJ_FL3[2, -8:] #Take the z_component
fJ_FR3 = pin.frameJacobian(solo.model, solo.data, q, ID_FR)[:3, -8:]
oJ_FR3 = oR_FR * fJ_FR3
oJ_FRz = oJ_FR3[2, -8:]
fJ_HL3 = pin.frameJacobian(solo.model, solo.data, q, ID_HL)[:3, -8:]
oJ_HL3 = oR_HL * fJ_HL3
oJ_HLz = oJ_HL3[2, -8:]
fJ_HR3 = pin.frameJacobian(solo.model, solo.data, q, ID_HR)[:3, -8:]
oJ_HR3 = oR_HR * fJ_HR3
oJ_HRz = oJ_HR3[2, -8:]
# Displacement and posture error
nu = np.vstack(
[err_FL[2], err_FR[2], err_HL[2], err_HR[2], omega * err_post[7:]])
# Making a single z-row Jacobian vector plus the posture Jacobian
J = np.vstack([oJ_FLz, oJ_FRz, oJ_HLz, oJ_HRz, omega * J_post])
# Computing the velocity
vq_act = -Kp * pinv(J) * nu
vq = np.concatenate((np.zeros([6, 1]), vq_act))
# Computing the updated configuration
q = pin.integrate(solo.model, q, vq * dt)
#hist_err.append(np.linalg.norm(nu))
#hist_err.append(err_post[7:])
########################################################################
# PD Torque controller
Kp_PD = 0.05
Kd_PD = 80 * Kp_PD
#Kd = 2 * np.sqrt(2*Kp) # formula to get a critical damping
torques = Kd_PD * (qdes[7:] - q[7:]) - Kp_PD * v[6:]
# Saturation to limit the maximal torque
t_max = 5
torques = np.maximum(np.minimum(torques, t_max * np.ones((8, 1))),
-t_max * np.ones((8, 1)))
return torques, stop
pass
place('world/framegoal', Mgoal)
place('world/yaxis',
pinocchio.SE3(rotate('x', np.pi / 2),
np.matrix([0, 0, .1]).T))
# Define robot initial configuration
q = robot.rand()
q[:2] = 0 # Basis at the center of the world.
DT = 1e-2 # Integration step.
# Loop on an inverse kinematics for 200 iterations.
for i in range(200): # Integrate over 1 second of robot life
pinocchio.forwardKinematics(robot.model, robot.data,
q) # Compute joint placements
pinocchio.updateFramePlacements(
robot.model, robot.data) # Also compute operational frame placements
Mtool = robot.data.oMf[
IDX_TOOL] # Get placement from world frame o to frame f oMf
J = pinocchio.frameJacobian(
robot.model, robot.data, q, IDX_TOOL,
pinocchio.ReferenceFrame.LOCAL) # Get corresponding jacobian
### ... YOUR CODE HERE
vq = rand(NV) # .... REPLACE THIS LINE BY YOUR CODE ...
### ... END OF YOUR CODE HERE
q = robot.integrate(q, vq * DT)
robot.display(q)
time.sleep(DT)
def c_walking_IK(q, qdot, dt, solo, t_simu):
# unactuated, [x, y, z] position of the base + [x, y, z, w] orientation of the base (stored as a quaternion)
# qu = q[:7]
qa = q[7:] # actuated, [q1, q2, ..., q8] angular position of the 8 motors
# [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 = 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.2 # 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.03 # displacement amplitude by x
dz = 0.06 # 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.frameJacobian(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.frameJacobian(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.frameJacobian(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.frameJacobian(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 = 8.
Kd = 0.2
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
def frameJacobian(self, q, index, update_geometry=True, local_frame=True):
if local_frame:
return se3.frameJacobian(self.model, self.data, index, q)
else:
pass
def callback_torques():
global v_prev, solo, stop, q, qdes, t
t_start = time()
jointStates = p.getJointStates(robotId,
revoluteJointIndices) # State of all joints
baseState = p.getBasePositionAndOrientation(robotId)
baseVel = p.getBaseVelocity(robotId)
# Info about contact points with the ground
contactPoints_FL = p.getContactPoints(robotId, planeId,
linkIndexA=2) # Front left foot
contactPoints_FR = p.getContactPoints(robotId, planeId,
linkIndexA=5) # Front right foot
contactPoints_HL = p.getContactPoints(robotId, planeId,
linkIndexA=8) # Hind left foot
contactPoints_HR = p.getContactPoints(robotId, planeId,
linkIndexA=11) # Hind right foot
# Sort contacts points to get only one contact per foot
contactPoints = []
contactPoints.append(getContactPoint(contactPoints_FL))
contactPoints.append(getContactPoint(contactPoints_FR))
contactPoints.append(getContactPoint(contactPoints_HL))
contactPoints.append(getContactPoint(contactPoints_HR))
# Joint vector for Pinocchio
q = np.vstack(
(np.array([baseState[0]]).transpose(), np.array([baseState[1]
]).transpose(),
np.array([[
jointStates[i_joint][0] for i_joint in range(len(jointStates))
]]).transpose()))
v = np.vstack(
(np.array([baseVel[0]]).transpose(), np.array([baseVel[1]
]).transpose(),
np.array([[
jointStates[i_joint][1] for i_joint in range(len(jointStates))
]]).transpose()))
v_dot = (v - v_prev) / h
v_prev = v.copy()
# Update display in Gepetto-gui
solo.display(q)
########################################################################
### Space mouse configuration : exit the loop when 0 is pressed and released
event = sp.poll() # return the next event if there is any
# if event signals the release of the first button
# exit condition : the button 0 is pressed and released
if type(event
) is sp.ButtonEvent and event.button == 0 and event.pressed == 0:
# set exit condition
stop = True
### Stack of Task : walking
#compute/update all the joints and frames
pin.forwardKinematics(solo.model, solo.data, qdes)
pin.updateFramePlacements(solo.model, solo.data)
# Getting 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
t1 = t #previous time
t += dt
t2 = t #current time
xzdes_FL = ftraj(t, xF0, zF0)
xzdes_HR = ftraj(t, xH0, zH0)
xzdes_FR = ftraj(t + T / 2, xF0, zF0)
xzdes_HL = ftraj(t + T / 2, xH0, zH0)
# 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.frameJacobian(solo.model, solo.data, qdes,
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.frameJacobian(solo.model, solo.data, qdes, ID_FR)[:3, -8:]
oJ_FR3 = oR_FR * fJ_FR3
oJ_FRxz = oJ_FR3[0::2, -8:]
fJ_HL3 = pin.frameJacobian(solo.model, solo.data, qdes, ID_HL)[:3, -8:]
oJ_HL3 = oR_HL * fJ_HL3
oJ_HLxz = oJ_HL3[0::2, -8:]
fJ_HR3 = pin.frameJacobian(solo.model, solo.data, qdes, 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
vq_act = -Kp * pinv(J) * nu
vq = np.concatenate((np.zeros([6, 1]), vq_act))
# Computing the updated configuration
qdes = pin.integrate(solo.model, qdes, vq * dt)
#hist_err.append(np.linalg.norm(nu))
########################################################################
# PD Torque controller
Kp_PD = 0.05
Kd_PD = 120 * Kp_PD
#Kd = 2 * np.sqrt(2*Kp) # formula to get a critical damping
torques = Kd_PD * (qdes[7:] - q[7:]) - Kp_PD * v[6:]
# Saturation to limit the maximal torque
t_max = 5
torques = np.maximum(np.minimum(torques, t_max * np.ones((8, 1))),
-t_max * np.ones((8, 1)))
# Control loop of 1/dt Hz
while (time() - t_start) < dt:
sleep(10e-6)
return torques, stop
pin.forwardKinematics(robot.model, robot.data, q)
pin.updateFramePlacements(robot.model, robot.data)
Mtool = robot.data.oMf[IDX_TOOL]
J = pin.frameJacobian(robot.model, robot.data, q, IDX_TOOL)
nu = pin.log(Mtool.inverse() * Mgoal).vector
vq = pinv(J)*nu
q = pin.integrate(robot.model, q, vq * dt)
robot.display(q)
sleep(dt)"""
### Position the basis on the line
robot.viewer.gui.addCylinder('world/yaxis', .01, 20, [.1, .1, .1, 1.])
place('world/yaxis', pin.SE3(rotate('x', np.pi / 2), np.matrix([0, 0, .1]).T))
IDX_BASIS = robot.model.getFrameId('base')
q_basis = q
for j in range(200):
pin.forwardKinematics(robot.model, robot.data, q_basis)
pin.updateFramePlacements(robot.model, robot.data)
Mbasis = robot.data.oMf[IDX_BASIS]
error = Mbasis.translation[0] - 1
J_basis = pin.frameJacobian(robot.model, robot.data, q, IDX_BASIS)[0, :]
nu_basis = error
vq_basis = pinv(J_basis) * nu_basis
q_basis = pin.integrate(robot.model, q_basis, vq_basis * dt)
robot.display(q_basis)
#sleep(dt)
embed()