def addRobot(self, robot):
if(ENABLE_VIEWER):
self.robots[robot.name] = robot
nodeName = "world/"+robot.name
print "adding robot: "+robot.name
self.loadDisplayModel(nodeName, "pinocchio", self.robots[robot.name]);
se3.framesKinematics(self.robots[robot.name].model,
self.robots[robot.name].data,
self.robots[robot.name].q0)
self.updateRobotConfig(self.robots[robot.name].q0,robot.name)
def display(self, q):
RobotWrapper.display(self, q)
M1 = self.data.oMi[1]
self.viewer.gui.applyConfiguration('world/mobilebasis', pin.se3ToXYZQUATtuple(M1))
self.viewer.gui.applyConfiguration('world/mobilewheel1', pin.se3ToXYZQUATtuple(M1))
self.viewer.gui.applyConfiguration('world/mobilewheel2', pin.se3ToXYZQUATtuple(M1))
self.viewer.gui.refresh()
pin.framesKinematics(self.model, self.data)
self.viewer.gui.applyConfiguration('world/framebasis', pin.se3ToXYZQUATtuple(self.data.oMf[-2]))
self.viewer.gui.applyConfiguration('world/frametool', pin.se3ToXYZQUATtuple(self.data.oMf[-1]))
self.viewer.gui.refresh()
def display(self, q):
RobotWrapper.display(self, q)
M1 = self.data.oMi[1]
self.viewer.gui.applyConfiguration('world/mobilebasis', M2gv(M1))
self.viewer.gui.applyConfiguration('world/mobilewheel1', M2gv(M1))
self.viewer.gui.applyConfiguration('world/mobilewheel2', M2gv(M1))
self.viewer.gui.refresh()
se3.framesKinematics(self.model, self.data)
self.viewer.gui.applyConfiguration('world/framebasis',
M2gv(self.data.oMf[-2]))
self.viewer.gui.applyConfiguration('world/frametool',
M2gv(self.data.oMf[-1]))
self.viewer.gui.refresh()
def update(self, q):
se3.computeAllTerms(self.model, self.data, self.q, self.v)
#se3.forwardKinematics(self.model, self.data, q, self.v, self.a)
#- se3::forwardKinematics
#- se3::crba
#- se3::nonLinearEffects
#- se3::computeJacobians
#- se3::centerOfMass
#- se3::jacobianCenterOfMass
#- se3::kineticEnergy
#- se3::potentialEnergy
se3.framesKinematics(self.model, self.data, q)
se3.computeJacobians(self.model, self.data, q)
se3.rnea(self.model, self.data, q, self.v, self.a)
self.biais(self.q, self.v)
self.q = q.copy()
def computePosition(self, q):
pin.framesKinematics(self.robot.model, self.robot.data, q)
frame_id = self.robot.model.getFrameId('lf_foot')
self.pos['lf_foot'] = self.getFramePosition(frame_id).translation
frame_id = self.robot.model.getFrameId('lh_foot')
self.pos['lh_foot'] = self.getFramePosition(frame_id).translation
frame_id = self.robot.model.getFrameId('rf_foot')
self.pos['rf_foot'] = self.getFramePosition(frame_id).translation
frame_id = self.robot.model.getFrameId('rh_foot')
self.pos['rh_foot'] = self.getFramePosition(frame_id).translation
return self.pos
def step(self, q, vq, tauq, dt=None):
if dt is None: dt = self.dt
robot = self.robot
NQ, NV, NB, RF, LF, RK, LK = self.NQ, self.NV, self.NB, self.RF, self.LF, self.RK, self.LK
# --- Simu
se3.forwardKinematics(robot.model, robot.data, q, v)
se3.framesKinematics(robot.model, robot.data)
Mrf = self.Mrf0.inverse() * robot.data.oMf[RF]
vrf = robot.model.frames[RF].placement.inverse() * robot.data.v[RK]
Mlf = self.Mlf0.inverse() * robot.data.oMf[LF]
vlf = robot.model.frames[LF].placement.inverse() * robot.data.v[LK]
qrf = np.vstack(
[Mrf.translation[1:],
se3.rpy.matrixToRpy(Mrf.rotation)[0]])
qlf = np.vstack(
[Mlf.translation[1:],
se3.rpy.matrixToRpy(Mlf.rotation)[0]])
frf = self.frf # Buffer where to store right force
frf[[1, 2, 3]] = self.Krf * qrf # Right force in effector frame
rf0_frf = se3.Force(frf) # Force in rf0 frame
rk_frf = (robot.data.oMi[RK].inverse() * self.Mrf0).act(
rf0_frf) # Spring force in R-knee frame.
flf = self.flf # Buffer where to store left force
flf[[1, 2, 3]] = self.Klf * qlf # Left force in effector frame
lf0_flf = se3.Force(flf) # Force in lf0 frame
lk_flf = (robot.data.oMi[LK].inverse() * self.Mlf0).act(
lf0_flf) # Spring force in L-knee frame.
self.forces = ForceDict({
RK: rk_frf,
LK: lk_flf
}, NB) # Argument to ABA
aq = se3.aba(robot.model, robot.data, q, vq, tauq, self.forces)
vq += aq * dt
q = se3.integrate(robot.model, q, vq * dt)
self.aq = aq
return q, vq
def __init__(self, robot, q0=None, dt=1e-3, ndt=10):
'''
Initialize from a Hrp2Reduced robot model, an initial configuration,
a timestep dt and a number of Eurler integration step ndt.
The <simu> method (later defined) processes <ndt> steps, each of them lasting <dt>/<ndt> seconds,
(i.e. total integration time when calling simu is <dt> seconds).
<q0> should be an attribute of robot if it is not given.
'''
self.frf = zero(6)
self.flf = zero(6)
self.dt = dt # Time step
self.ndt = ndt # Discretization (number of calls of step per time step)
self.robot = robot
self.NQ = robot.model.nq
self.NV = robot.model.nv
self.NB = robot.model.nbodies
self.RF = robot.model.getFrameId('rankle')
self.LF = robot.model.getFrameId('lankle')
self.RK = robot.model.frames[self.RF].parent
self.LK = robot.model.frames[self.LF].parent
NQ, NV, NB, RF, LF, RK, LK = self.NQ, self.NV, self.NB, self.RF, self.LF, self.RK, self.LK
if q0 is None: q0 = robot.q0
se3.forwardKinematics(robot.model, robot.data, q0)
se3.framesKinematics(robot.model, robot.data)
self.Mrf0 = robot.data.oMf[RF].copy(
) # Initial (i.e. 0-load) position of the R spring.
self.Mlf0 = robot.data.oMf[LF].copy(
) # Initial (i.e. 0-load) position of the L spring.
self.Krf = -np.diagflat([20000., 200000., 00.
]) # Stiffness of the Right spring
self.Klf = -np.diagflat([20000., 200000., 00.
]) # Stiffness of the Left spring
robot.viewer.addXYZaxis('world/mrf', [1., .6, .6, 1.], .03, .1)
robot.viewer.addXYZaxis('world/mlf', [.6, .6, 1., 1.], .03, .1)
robot.viewer.applyConfiguration('world/mrf', se3ToXYZQUAT(self.Mrf0))
robot.viewer.applyConfiguration('world/mlf', se3ToXYZQUAT(self.Mlf0))
def inverse_kinematics(self, fid, xdes, q0):
"""
Method not in use right now, but is here with the intention
of using pinocchio for inverse kinematics instead of using
the in-house IK solver of pybullet.
"""
raise NotImplementedError()
dt = 1.0e-3
pinocchio.computeJointJacobians(
self.robot_model,
self.data,
q0,
)
pinocchio.framesKinematics(
self.robot_model,
self.data,
q0,
)
pinocchio.framesForwardKinematics(
self.robot_model,
self.data,
q0,
)
Ji = pinocchio.getFrameJacobian(
self.robot_model,
self.data,
fid,
pinocchio.ReferenceFrame.LOCAL_WORLD_ALIGNED,
)[:3, :]
xcurrent = self.data.oMf[fid].translation
try:
Jinv = np.linalg.inv(Ji)
except Exception:
Jinv = np.linalg.pinv(Ji)
dq = Jinv.dot(xdes - xcurrent)
qnext = pinocchio.integrate(self.robot_model, q0, dt * dq)
return qnext
def tip(self, q):
'''Return the altitude of pendulum tip'''
se3.framesKinematics(self.model, self.data, q)
return self.data.oMf[1].translation[2, 0]
def framesKinematics(self, q):
se3.framesKinematics(self.model, self.data, q)
def compute_ddf_approximations(robot):
''' Compute ddf based on the following dynamics equation:
ddf_1 = K*(-Upsilon*f - dJ*v + J*M_inv*h - J*Minv*S^T*tau)
'''
ddf_list = {}
for i in range(1,11):
ddf_list[str(i)] = matlib.empty((nf,T))*np.nan
dyn_res = matlib.empty((nv,T))*np.nan # residual of robot dynamics
S = matlib.zeros((na,nv))
S[:,nv-na:] = matlib.eye(na)
dq_cmd_integral = q_cmd[:,0].copy()
eig_J_Minv_ST_Mj_diag_JSTpinv = np.empty((nf,T))
for t in range(T):
se3.computeAllTerms(robot.model, robot.data, q[:,t], v[:,t])
se3.framesKinematics(robot.model, robot.data, q[:,t])
M = robot.data.M #(7,7)
h = robot.data.nle
Jl,Jr = robot.get_Jl_Jr_world(q[:,t], False)
J = np.vstack([Jl[1:3],Jr[1:3]]) # (4, 7)
driftLF,driftRF = robot.get_driftLF_driftRF_world(q[:,t], v[:,t], False)
dJ_v = np.vstack([driftLF.vector[1:3], driftRF.vector[1:3]])
Mj = robot.data.M[3:,3:] #(7,7)
Minv = np.linalg.inv(M)
Upsilon = J*Minv*J.T
ddf_list['1'][:,t] = K*(-Upsilon*f[:,t] - dJ_v + J*Minv*h - J*Minv*S.T*tau[:,t])
tau_2 = Kp_pos*(q_cmd[:,t]-q[4:,t]) - Kd_pos*v[3:,t]
ddf_list['2'][:,t] = K*(-Upsilon*f[:,t] - dJ_v + J*Minv*h - J*Minv*S.T*tau_2)
# simplified version of controller (without postural task)
feet_v_ref = - Kf*(f_des[:,t] - f[:,t])
JSTpinv = np.linalg.pinv(J*S.T)
dq_cmd = JSTpinv * feet_v_ref
dq_cmd_integral += dt*dq_cmd
if(t==0): dq_cmd_integral = q_cmd[:,0]
tau_3 = Kp_pos*(dq_cmd_integral-q[4:,t]) - Kd_pos*v[3:,t]
ddf_list['3'][:,t] = K*(-Upsilon*f[:,t] - dJ_v + J*Minv*h - J*Minv*S.T*tau_3)
# compute eigenvalues of J*Minv*S.T*Mj_diag*JSTpinv to check its positive-definiteness
A = J*Minv*S.T*Mj_diag*JSTpinv
eig_J_Minv_ST_Mj_diag_JSTpinv[:,t] = eigvals(A)
B = Minv*S.T*Mj_diag*S
B_bar = B - matlib.eye(B.shape[0])
# move the pseudoinverse out of the integral
if(t==0):
feet_v_ref_integral_0 = J*S.T*q_cmd[:,0]
e_f_integral_0 = Kf_inv*feet_v_ref_integral_0
feet_v_ref_integral = feet_v_ref_integral_0
else:
feet_v_ref_integral += dt*feet_v_ref
tau_4 = Kp_pos*(JSTpinv*feet_v_ref_integral - q[4:,t]) - Kd_pos*v[3:,t]
ddf_list['4'][:,t] = K*(-Upsilon*f[:,t] - dJ_v + J*Minv*h - J*Minv*S.T*tau_4)
# equivalent reformulation
tau_5 = kp_bar*A*feet_v_ref_integral - kp_bar*J*B*q[1:,t] + kd_bar*K_inv*df[:,t] - kd_bar*J*B_bar*v[:,t]
ddf_list['5'][:,t] = K*(-Upsilon*f[:,t] - dJ_v + J*Minv*h - tau_5)
# remove dJ_v
e_f_integral = Kf_inv*feet_v_ref_integral
tau_6 = kp_bar*A*Kf*e_f_integral - kp_bar*J*B*q[1:,t] + kd_bar*K_inv*df[:,t] - kd_bar*J*B_bar*v[:,t]
ddf_list['6'][:,t] = K*(-Upsilon*f[:,t] + J*Minv*h - tau_6)
# equivalent reformulation
ddf_list['7'][:,t] = -kd_bar*df[:,t] -K*Upsilon*f[:,t] -kp_bar*K*A*Kf*e_f_integral +K*J*Minv*h +kp_bar*K*J*B*q[1:,t] +kd_bar*K*J*B_bar*v[:,t]
# neglect B_bar
ddf_list['8'][:,t] = -kd_bar*df[:,t] -K*Upsilon*f[:,t] -kp_bar*K*A*Kf*e_f_integral +K*J*Minv*h +kp_bar*K*J*B*q[1:,t]
# neglect J*B*q
ddf_list['9'][:,t] = -kd_bar*df[:,t] -K*Upsilon*f[:,t] -kp_bar*K*A*Kf*e_f_integral +K*J*Minv*h +kd_bar*K*J*B_bar*v[:,t]
# neglect K*J*Minv*h and assume zero integral at start
ddf_list['10'][:,t] = -kd_bar*df[:,t] -K*Upsilon*f[:,t] -kp_bar*K*A*Kf*(e_f_integral-e_f_integral_0) +kp_bar*K*J*B*q[1:,t] +kd_bar*K*J*B_bar*v[:,t]
dyn_res[:,t] = M*dv[:,t] + h - J.T*f[:,t] - S.T*tau[:,t]
if(t*dt>=T_DISTURB_BEGIN and t*dt<=T_DISTURB_END):
ddf[:,t] = np.nan
dyn_res[:,t] = np.nan
for key in ddf_list:
ddf_list[key][:,t] = np.nan
# compute errors
for key in np.sort(ddf_list.keys()):
print "Max error ddf[%s]: %.0f"%(key, np.nanmax(np.abs(ddf-ddf_list[key])))
ncols = 2
nsubplots = nf
nrows = int(nsubplots/ncols)
for key in np.sort(ddf_list.keys()):
ff, ax = plt.subplots(nrows, ncols, sharex=True);
ax = ax.reshape(nsubplots)
for i in range(nf):
ax[i].plot(time, ddf[i,:].A1, '-', label='Real')
ax[i].plot(time, ddf_list[key][i,:].A1, '--', label='Approx '+key)
ax[i].legend()
ax[i].set_title('ddf '+str(i))
# ax[i].set_ylabel('N/s*s')
ax[-1].set_xlabel('Time [s]')
if(PLOT_EIGENVALUES_J_Minv_ST_Mj_diag_JSTpinv):
plt.figure()
plt.plot(eig_J_Minv_ST_Mj_diag_JSTpinv.T)
plt.title('Eigenvalues of J*Minv*S.T*Mj_diag*(J*S.T)^+')
if(PLOT_DYNAMICS_RESIDUAL):
ff, ax = plt.subplots(1, 1, sharex=True);
for i in range(nv):
ax.plot(time, dyn_res[i,:].A1, '-', label='Axis '+str(i))
ax.set_title('Dynamics residual')
ax.set_ylabel('Nm')
ax.set_xlabel('Time [s]')
ax.legend()
return ddf_list
def tip(self,q):
'''Return the altitude of pendulum tip'''
se3.framesKinematics(self.model,self.data,q)
return self.data.oMf[1].translation[2,0]
pkg = '/home/student/models/'
urdf = pkg + 'ur_description/urdf/ur5_gripper.urdf'
robot = MobileRobotWrapper(urdf, [
pkg,
])
robot.initDisplay(loadModel=True)
#robot.viewer.gui.addFloor('world/floor')
NQ = robot.model.nq
NV = robot.model.nv
q = se3.randomConfiguration(robot.model, zero(NQ) - np.pi, zero(NQ) + np.pi)
vq = rand(NV)
robot.display(q)
from time import sleep
for i in range(10000):
robot.increment(q, vq / 100)
robot.display(q)
sleep(.01)
IDX_TOOL = 24
IDX_BASIS = 23
se3.framesKinematics(robot.model, robot.data)
Mtool = robot.data.oMf[IDX_TOOL]
Mbasis = robot.data.oMf[IDX_BASIS]
def updateRobotConfig(self, q):
se3.computeAllTerms(self.robot.model, self.robot.data, q, self.robot.v)
se3.framesKinematics(self.robot.model, self.robot.data, q)
se3.computeJacobians(self.robot.model, self.robot.data, q)
