def df_dq(model,func,q,h=1e-9):
""" Perform df/dq by num_diff. q is in the lie manifold.
:params func: function to differentiate f : np.matrix -> np.matrix
:params q: configuration value at which f is differentiated. type np.matrix
:params h: eps
:returns df/dq
"""
dq = zero(model.nv)
f0 = func(q)
res = zero([len(f0),model.nv])
for iq in range(model.nv):
dq[iq] = h
res[:,iq] = (func(pin.integrate(model,q,dq)) - f0)/h
dq[iq] = 0
return res
def callback_torques():
global init, sol, t, v_prev, q, qdes, vdes, trajCom, trajFLfoot, trajFRfoot, trajHLfoot, trajHRfoot, comTask, FLfootTask, FRfootTask, HLfootTask, HRfootTask
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) / dt
v_prev = v.copy()
####################################################################
pin.forwardKinematics(model, data, q)
HL_foot_ref = robot.framePosition(data, model.getFrameId('HL_FOOT'))
HR_foot_ref = robot.framePosition(data, model.getFrameId('HR_FOOT'))
FL_foot_ref = robot.framePosition(data, model.getFrameId('FL_FOOT'))
FR_foot_ref = robot.framePosition(data, model.getFrameId('FR_FOOT'))
base = robot.framePosition(data, model.getFrameId('base_link'))
tHL = HL_foot_ref.translation
tHR = HR_foot_ref.translation
tFL = FL_foot_ref.translation
tFR = FR_foot_ref.translation
tbase = base.translation
ztFR = tFR[2, 0]
zGoal = tbase[
2, 0] - 0.235 # altitude objective : 0 from basis reference frame
print(tbase[2, 0])
FR_foot_goal = FR_foot_ref.copy()
FR_foot_goal.translation = np.matrix([tFR[0, 0], tFR[1, 0], zGoal]).T
FL_foot_goal = FL_foot_ref.copy()
FL_foot_goal.translation = np.matrix([tFL[0, 0], tFL[1, 0], zGoal]).T
HR_foot_goal = HR_foot_ref.copy()
HR_foot_goal.translation = np.matrix([tHR[0, 0], tHR[1, 0], zGoal]).T
HL_foot_goal = HL_foot_ref.copy()
HL_foot_goal.translation = np.matrix([tHL[0, 0], tHL[1, 0], zGoal]).T
trajHLfoot = tsid.TrajectorySE3Constant("traj_HL_foot", HL_foot_goal)
trajHRfoot = tsid.TrajectorySE3Constant("traj_HR_foot", HR_foot_goal)
trajFLfoot = tsid.TrajectorySE3Constant("traj_FL_foot", FL_foot_goal)
trajFRfoot = tsid.TrajectorySE3Constant("traj_FR_foot", FR_foot_goal)
samplePosture = trajPosture.computeNext()
postureTask.setReference(samplePosture)
sampleHLfoot = trajHLfoot.computeNext()
HLfootTask.setReference(sampleHLfoot)
sampleHRfoot = trajHRfoot.computeNext()
HRfootTask.setReference(sampleHRfoot)
sampleFLfoot = trajFLfoot.computeNext()
FLfootTask.setReference(sampleFLfoot)
sampleFRfoot = trajFRfoot.computeNext()
FRfootTask.setReference(sampleFRfoot)
HQPData = invdyn.computeProblemData(t, qdes, vdes)
sol = solver.solve(HQPData)
tau = invdyn.getActuatorForces(sol)
dv = invdyn.getAccelerations(sol)
v_mean = vdes + 0.5 * dt * dv
vdes += dt * dv
qdes = pin.integrate(model, qdes, dt * v_mean)
t += dt
robot_display.display(q)
qlist.append(q)
####################################################################
# PD Torque controller
Kp_PD = 10.
Kd_PD = 0.1
#Kd = 2 * np.sqrt(2*Kp) # formula to get a critical damping
torques = Kp_PD * (qdes[7:] - q[7:]) + Kd_PD * (vdes[6:] - v[6:])
# Saturation to limit the maximal torque
t_max = 5
torques = np.maximum(np.minimum(torques, t_max * np.ones((12, 1))),
-t_max * np.ones((12, 1)))
return torques
def vq2q(self, vq):
return pinocchio.integrate(rmodel, self.q0, vq)
import pinocchio
model = pinocchio.buildSampleModelManipulator()
data = model.createData()
JOINT_ID = 6
xdes = np.matrix([ 0.5,-0.5,0.5]).T
q = model.neutralConfiguration
eps = 1e-4
IT_MAX = 1000
DT = 1e-1
for i in range(IT_MAX):
pinocchio.forwardKinematics(model,data,q)
x = data.oMi[JOINT_ID].translation
R = data.oMi[JOINT_ID].rotation
err = R.T*(x-xdes)
if np.linalg.norm(err) < eps:
print("Convergence achieved!")
break
J = pinocchio.jointJacobian(model,data,q,JOINT_ID,pinocchio.ReferenceFrame.LOCAL,True)[:3,:]
v = - np.linalg.pinv(J)*err
q = pinocchio.integrate(model,q,v*DT)
if not i % 10: print('error = %s' % (x-xdes).T)
else:
print("\nWarning: the iterative algorithm has not reached convergence to the desired precision")
print('\nresult: %s' % q.flatten().tolist())
print('\nfinal error: %s' % (x-xdes).T)
dcrba = DCRBA(robot)
dcrba.pre(q)
Mp = dcrba()
# --- Validate dM/dq by finite diff
dM = np.zeros([robot.model.nv,]*3)
eps = 1e-6
dq = zero(robot.model.nv)
for diff in range(robot.model.nv):
dM[:,:,diff] = -pin.crba(robot.model,robot.data,q)
dq *=0; dq[diff] = eps
qdq = pin.integrate(robot.model,q,dq)
dM[:,:,diff] += pin.crba(robot.model,robot.data,qdq)
dM /= eps
print('Check dCRBA = \t\t\t', max([ norm(Mp[:,:,diff]-dM[:,:,diff]) for diff in range(robot.model.nv) ]))
# --- vRNEA ---
# --- vRNEA ---
# --- vRNEA ---
vrnea = VRNEA(robot)
Q = vrnea(q)
def update_configuration(self, delta_q):
self.q = pinocchio.integrate(self.model, self.q, delta_q)
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.
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
sol.status)
break
tau[:, i] = formulation.getActuatorForces(sol)
dv[:, i] = formulation.getAccelerations(sol)
dv_des[:, i] = postureTask.getDesiredAcceleration
if i % conf.PRINT_N == 0:
print("Time %.3f" % (t))
print("\ttracking err %s: %.3f" % (postureTask.name.ljust(
20, '.'), norm(postureTask.position_error, 2)))
# numerical integration
v_mean = v[:, i] + 0.5 * dt * dv[:, i]
v[:, i + 1] = v[:, i] + dt * dv[:, i]
q[:, i + 1] = se3.integrate(model, q[:, i], dt * v_mean)
t += conf.dt
if i % conf.DISPLAY_N == 0:
robot_display.display(q[:, i])
time_spent = time.time() - time_start
if (time_spent < conf.dt): time.sleep(conf.dt - time_spent)
# PLOT STUFF
time = np.arange(0.0, N * conf.dt, conf.dt)
if (PLOT_JOINT_POS):
(f, ax) = plut.create_empty_figure(int(robot.nv / 2), 2)
ax = ax.reshape(robot.nv)
for i in range(robot.nv):
vq = rand(robot.model.nv)
aq = rand(robot.model.nv)
tau = rand(robot.model.nv) # Motor torque
#
"""
M(q) ddq + C(q, dq) + G(q) = Tau
b = C(q, dq) + G(q) (bias term as in Featherstone)
"""
# compute dynamic drift -- Coriolis, centrifugal, gravity
b = pin.rnea(robot.model, robot.data, q, vq, np.zeros_like(aq))
# compute mass matrix M
M = pin.crba(robot.model, robot.data, q)
ddq = np.linalg.solve(M, (tau - b)) # valid only when no collision
dt, dq = 0.01, 0
dq += ddq * dt
q = pin.integrate(robot.model, q, dq * dt)
#
# M = pin.crba(robot, robot.data, q)
# # # d/dq M(q)
# dcrba = DCRBA(robot)
# dcrba.pre(q)
# Mp = dcrba()
# #
# # d/dvq RNEA(q,vq) = C(q,vq)
# coriolis = Coriolis(robot)
# C = coriolis(q,vq)
# #
# # # d/dq RNEA(q,vq,aq)
# drnea = DRNEA(robot)
# R = drnea(q,vq,aq)
# print('hi')
def integrate(self,q,vq):
'''
Given a configuration q and displacement dq = vq*dt, returns the integration
of this displacement, i.e. something that looks like q+dq.
'''
return pinocchio.integrate(self.model,q,vq)
import numpy as np
import scipy as sp
import time
PKG = '/opt/openrobots/share'
URDF = join(PKG, 'ur_description/urdf/ur5_gripper.urdf')
robot = RobotWrapper(URDF, [PKG])
robot.initDisplay(loadModel=True)
#Initial Set up & Variables
q = rand(robot.nq)
vq = zero(robot.nv)
aq = zero(robot.nv)
se3.forwardKinematics(robot.model, robot.data, q)
robot.display(q)
input_specified = None
dt = 0.001
while 1:
if input_specified:
pass
else:
tau = np.full((1, robot.nv), 10).T
#tau = zero(robot.nv)
aq = se3.aba(robot.model, robot.data, q, vq, tau)
vq += aq * dt
q = se3.integrate(robot.model, q, vq * dt)
robot.display(q)
Jpinv = np.zeros((robot.nv, 3))
error_past = 1e100
robot.v = np.zeros(robot.nv)
for i in range(0, max_it):
se3.computeAllTerms(model, data, robot.q, robot.v)
sample = traj.computeNext()
taskCOM.setReference(sample)
const = taskCOM.compute(t, robot.q, robot.v)
Jpinv = np.linalg.pinv(const.getMatrix(), 1e-5)
dv = np.dot(Jpinv, const.getVector().T)
assert np.linalg.norm(np.dot(Jpinv, const.getMatrix()), 2) - 1.0 < tol
robot.v += dt * dv
robot.q = se3.integrate(model, robot.q, dt * robot.v)
t += dt
error = np.linalg.norm(taskCOM.position_error(), 2)
assert error - error_past < 1e-4
error_past = error
if error < 1e-8:
print("Success Convergence")
break
if i % 100 == 0:
print("Time :", t, "COM pos error :", error, "COM vel error :",
np.linalg.norm(taskCOM.velocity_error(), 2))
print("")
print("Test Task Joint Posture")
print("")
sol.status)
break
tau[:, i] = formulation.getActuatorForces(sol)
dv[:, i] = formulation.getAccelerations(sol)
dv_des[:, i] = postureTask.getDesiredAcceleration
if i % conf.PRINT_N == 0:
print("Time %.3f" % (t))
print("\ttracking err %s: %.3f" % (postureTask.name.ljust(
20, '.'), norm(postureTask.position_error, 2)))
# numerical integration
v_mean = v[:, i] + 0.5 * dt * dv[:, i]
v[:, i + 1] = v[:, i] + dt * dv[:, i]
q[:, i + 1] = pin.integrate(model, q[:, i], dt * v_mean)
t += conf.dt
if i % conf.DISPLAY_N == 0:
robot_display.display(q[:, i])
time_spent = time.time() - time_start
if (time_spent < conf.dt): time.sleep(conf.dt - time_spent)
# PLOT STUFF
time = np.arange(0.0, N * conf.dt, conf.dt)
if (PLOT_JOINT_POS):
(f, ax) = plut.create_empty_figure(int(robot.nv / 2), 2)
ax = ax.reshape(robot.nv)
for i in range(robot.nv):
model.lowerPositionLimit.fill(-math.pi)
model.upperPositionLimit.fill(+math.pi)
if args.with_cart:
model.lowerPositionLimit[0] = model.upperPositionLimit[0] = 0.
data_sim = model.createData()
t = 0.
q = pin.randomConfiguration(model)
v = np.zeros((model.nv))
tau_control = np.zeros((model.nv))
damping_value = 0.1
for k in range(N):
tic = time.time()
tau_control = -damping_value * v # small damping
a = pin.aba(model, data_sim, q, v, tau_control) # Forward dynamics
# Semi-explicit integration
v += a * dt
q = pin.integrate(model, q, v * dt) # Configuration integration
viz.display(q)
toc = time.time()
ellapsed = toc - tic
dt_sleep = max(0, dt - (ellapsed))
time.sleep(dt_sleep)
t += dt
tau_old = tau
if i % PRINT_N == 0:
print "Time ", i
if invdyn.checkContact(contactRF.name, sol):
f = invdyn.getContactForce(contactRF.name, sol)
print " ", contactRF.name, "force: ", contactRF.getNormalForce(
f)
if invdyn.checkContact(contactLF.name, sol):
f = invdyn.getContactForce(contactLF.name, sol)
print " ", contactLF.name, "force: ", contactLF.getNormalForce(
f)
print " ", comTask.name, " err:", norm(comTask.position_error, 2)
print " ", "v: ", norm(v, 2), "dv: ", norm(dv)
v += dt * dv
q = se3.integrate(model, q, dt * v)
t += dt
assert norm(dv) < 1e6
assert norm(v) < 1e6
data_2 = model.createData()
# robot.data()
com = se3.centerOfMass(model, data_2, q)
com_correction = com - comTask.position_error
# validate that it works
print "Final COM Position", com_correction.transpose()
print "Desired COM Position", com_ref.transpose()
Jpinv = np.matrix(np.zeros((robot.nv, 3)))
error_past = 1e100
v = np.matrix(np.zeros(robot.nv)).transpose()
for i in range(0, max_it):
robot.computeAllTerms(data, q, v)
sample = traj.computeNext()
taskCOM.setReference(sample)
const = taskCOM.compute(t, q, v, data)
Jpinv = np.linalg.pinv(const.matrix, 1e-5)
dv = Jpinv * const.vector
assert np.linalg.norm(Jpinv * const.matrix, 2) - 1.0 < tol
v += dt * dv
q = pin.integrate(model, q, dt * v)
t += dt
error = np.linalg.norm(taskCOM.position_error, 2)
assert error - error_past < 1e-4
error_past = error
if error < 1e-8:
print("Success Convergence")
break
if i % 100 == 0:
print("Time :", t, "COM pos error :", error, "COM vel error :",
np.linalg.norm(taskCOM.velocity_error, 2))
print("")
print("Test Task Joint Posture")
print("")
def c_walking(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
# Method : Inverse Kinematics
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
Kp = 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 = -Kp * 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:]
solo.display(q)
# End of the control code
###############################################
# Parameters for the PD controller
Kp = 8.
Kd = 0.2
torques_sat = 3 * np.ones((8, 1)) # 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, torques_sat,
torques_ref)
# torques must be a numpy array of shape (8, 1) containing the torques applied to the 8 motors
return torques
solo = robots_loader.loadSolo()
solo.initDisplay(loadModel=True)
q = solo.q0
solo.display(q)
vq = np.zeros([14, 1])
dt = 1e-2
# loop over space navigator events
while not stop:
# wait for next event
event = sp.wait()
# 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
# matching the mouse signals with the position of Solo's basis
if type(event) is sp.MotionEvent:
vq[3] = -event.rx / 100.0
vq[5] = event.ry / 100.0
#vq[4] = event.rz/100.0
q = pin.integrate(solo.model, q, vq * dt)
solo.display(q)
embed()
DT = 1e-1
damp = 1e-6
i = 0
while True:
pinocchio.forwardKinematics(model, data, q)
dMi = oMdes.actInv(data.oMi[JOINT_ID])
err = pinocchio.log(dMi).vector
if norm(err) < eps:
success = True
break
if i >= IT_MAX:
success = False
break
J = pinocchio.computeJointJacobian(model, data, q, JOINT_ID)
v = -J.T.dot(solve(J.dot(J.T) + damp * np.eye(6), err))
q = pinocchio.integrate(model, q, v * DT)
if not i % 10:
print('%d: error = %s' % (i, err.T))
i += 1
if success:
print("Convergence achieved!")
else:
print(
"\nWarning: the iterative algorithm has not reached convergence to the desired precision"
)
print('\nresult: %s' % q.flatten().tolist())
print('\nfinal error: %s' % err.T)
def step(self, u, dt=None):
if dt is None:
dt = self.dt
# (Forces are directly in the world frame, and aba wants them in the end effector frame)
se3.computeAllTerms(self.model, self.data, self.q, self.v)
se3.updateFramePlacements(self.model, self.data)
M = self.data.M
h = self.data.nle
self.collision_detection()
self.compute_forces(False)
if(self.simulate_coulomb_friction and self.simulation_type=='timestepping'):
# minimize kinetic energy using time stepping
from quadprog import solve_qp
'''
Solve a strictly convex quadratic program
Minimize 1/2 x^T G x - a^T x
Subject to C.T x >= b
Input Parameters
----------
G : array, shape=(n, n)
a : array, shape=(n,)
C : array, shape=(n, m) matrix defining the constraints
b : array, shape=(m), default=None, vector defining the constraints
meq : int, default=0
the first meq constraints are treated as equality constraints,
all further as inequality constraints
'''
# M (v' - v) = dt*S^T*(tau - tau_c) - dt*h + dt*J^T*f
# M v' = M*v + dt*(S^T*tau - h + J^T*f) - dt*S^T*tau_c
# M v' = b + B*tau_c
# v' = Minv*(b + B*tau_c)
b = M.dot(self.v) + dt*(self.S.T.dot(u) - h + self.Jc.T.dot(self.f))
B = - dt*self.S.T
# Minimize kinetic energy:
# min v'.T * M * v'
# min (b+B*tau_c).T*Minv*(b+B*tau_c)
# min tau_c.T * B.T*Minv*B* tau_C + 2*b.T*Minv*B*tau_c
Minv = np.linalg.inv(M)
G = B.T.dot(Minv.dot(B))
a = -b.T.dot(Minv.dot(B))
C = np.vstack((np.eye(self.na), -np.eye(self.na)))
c = np.concatenate((-self.tau_coulomb_max, -self.tau_coulomb_max))
solution = solve_qp(G, a, C.T, c, 0)
self.tau_c = solution[0]
self.v = Minv.dot(b + B.dot(self.tau_c))
self.q = se3.integrate(self.model, self.q, self.v*dt)
elif(self.simulation_type=='euler' or self.simulate_coulomb_friction==False):
self.tau_c = self.tau_coulomb_max*np.sign(self.v[-self.na:])
self.dv = np.linalg.solve(M, self.S.T.dot(u-self.tau_c) - h + self.Jc.T.dot(self.f))
v_mean = self.v + 0.5*dt*self.dv
self.v += self.dv*dt
self.q = se3.integrate(self.model, self.q, v_mean*dt)
else:
print("[ERROR] Unknown simulation type:", self.simulation_type)
self.t += dt
return self.q, self.v
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
def solve(self, dt, q_init, dq_init, com_ref, lmom_ref, amom_ref,
endeff_pos_ref, endeff_vel_ref, endeff_contact, joint_pos_ref,
base_ori_ref):
num_time_steps = com_ref.shape[0]
com_kin = np.zeros_like(com_ref)
lmom_kin = np.zeros_like(lmom_ref)
amom_kin = np.zeros_like(amom_ref)
endeff_pos_kin = np.zeros_like(endeff_pos_ref)
endeff_vel_kin = np.zeros_like(endeff_vel_ref)
q_kin = np.zeros([num_time_steps, q_init.shape[0]])
dq_kin = np.zeros([num_time_steps, dq_init.shape[0]])
ddq_kin = np.zeros_like(dq_kin)
inner_steps = int(dt / 0.001)
inner_dt = 0.001
time = np.linspace(0., (num_time_steps - 1) * dt, num_time_steps)
splined_com_ref = CubicSpline(time, com_ref)
splined_lmom_ref = CubicSpline(time, lmom_ref)
splined_amom_ref = CubicSpline(time, amom_ref)
splined_endeff_pos_ref = CubicSpline(time, endeff_pos_ref)
splined_endeff_vel_ref = CubicSpline(time, endeff_vel_ref)
splined_joint_pos_ref = CubicSpline(time, joint_pos_ref)
splined_base_ori_ref = CubicSpline(time, base_ori_ref)
# store the first one
q = q_init.copy()
dq = dq_init.copy()
self.update_kinematics(q, dq)
q_kin[0] = q
dq_kin[0] = dq
com_kin[0] = self.robot.com(q).T
hg = self.robot.centroidalMomentum(q, dq)
lmom_kin[0] = hg.linear.T
amom_kin[0] = hg.angular.T
endeff_pos_kin[0] = self.framesPos(self.endeff_ids)
endeff_vel_kin[0] = (self.J[6:(self.ne + 2) * 3].dot(dq).T).reshape(
[self.ne, 3])
dmom_ref = np.zeros([
6,
])
endeff_acc_ref = np.zeros([self.ne, 3])
t = 0.
for it in range(1, num_time_steps):
for inner in range(inner_steps):
dmom_ref = np.hstack(
(splined_lmom_ref(t, nu=1), splined_amom_ref(t, nu=1)))
endeff_acc_ref = splined_endeff_vel_ref(t, nu=1)
orien_ref = pin.Quaternion(
pin.rpy.rpyToMatrix(splined_base_ori_ref(t)))
ddq = self.step(
q, dq, splined_com_ref(t), orien_ref,
np.hstack((splined_lmom_ref(t), splined_amom_ref(t))),
dmom_ref, splined_endeff_pos_ref(t),
splined_endeff_vel_ref(t), endeff_acc_ref,
endeff_contact[it], splined_joint_pos_ref(t))
# Integrate to the next state.
dq += ddq * inner_dt
q = pin.integrate(self.robot.model, q, dq * inner_dt)
t += inner_dt
self.update_kinematics(q, dq)
q_kin[it] = q
dq_kin[it] = dq
ddq_kin[it] = ddq
com_kin[it] = self.robot.com(q).T
hg = self.robot.centroidalMomentum(q, dq)
lmom_kin[it] = hg.linear.T
amom_kin[it] = hg.angular.T
endeff_pos_kin[it] = self.framesPos(self.endeff_ids)
endeff_vel_kin[it] = (self.J[6:(self.ne + 2) *
3].dot(dq).T).reshape([self.ne, 3])
return q_kin, dq_kin, com_kin, lmom_kin, amom_kin, endeff_pos_kin, endeff_vel_kin
if i%PRINT_N == 0:
print "Time %.3f"%(t)
if invdyn.checkContact(contactRF.name, sol):
f = invdyn.getContactForce(contactRF.name, sol)
print "\tnormal force %s: %.1f"%(contactRF.name.ljust(20,'.'),contactRF.getNormalForce(f))
if invdyn.checkContact(contactLF.name, sol):
f = invdyn.getContactForce(contactLF.name, sol)
print "\tnormal force %s: %.1f"%(contactLF.name.ljust(20,'.'),contactLF.getNormalForce(f))
print "\ttracking err %s: %.3f"%(comTask.name.ljust(20,'.'), norm(comTask.position_error, 2))
print "\t||v||: %.3f\t ||dv||: %.3f"%(norm(v, 2), norm(dv))
v_mean = v + 0.5*dt*dv
v += dt*dv
q = se3.integrate(robot.model(), q, dt*v_mean)
t += dt
if i%DISPLAY_N == 0: robot_display.display(q)
time_spent = time.time() - time_start
if(time_spent < dt): time.sleep(dt-time_spent)
# PLOT STUFF
time = np.arange(0.0, N_SIMULATION*dt, dt)
(f, ax) = plut.create_empty_figure(3,1)
for i in range(3):
ax[i].plot(time, com_pos[i,:].A1, label='CoM '+str(i))
ax[i].plot(time, com_pos_ref[i,:].A1, 'r:', label='CoM Ref '+str(i))
ax[i].set_xlabel('Time [s]')
def integrate_dv(self, q, v, dv, dt):
v_mean = v + 0.5 * dt * dv
v += dt * dv
q = se3.integrate(self.model, q, dt * v_mean)
return q, v
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()
def increment(self, q, v, dt, t, updateViewer=True):
self.t = t
self.time_step += 1
self.robot.v = v.copy() * dt
self.q = se3.integrate(self.robot.model, q.copy(), self.robot.v)
self.viewer.updateRobotConfig(self.q, self.robotName)
