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
return np.matrix([x, z])
# Getting 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")
hist_err = [] #history of the error
# loop over space navigator events
while not stop:
### Space mouse configuration : exit the loop when 0 is pressed and released
# return the next event if there is any
event = sp.poll()
# if event signals the release of the first button
if type(event) is sp.ButtonEvent \
and event.button == 0 and event.pressed == 0:
# set exit condition
stop = True
### Stack of Task : walking
pin.forwardKinematics(solo.model, solo.data, q)
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]
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