def cost(self, x):
q = a2m(x)
pinocchio.forwardKinematics(self.rmodel, self.rdata, q)
pinocchio.updateFramePlacements(self.rmodel, self.rdata)
refMl = self.refL.inverse() * self.rdata.oMf[self.idL]
residualL = m2a(pinocchio.log(refMl).vector)
refMr = self.refR.inverse() * self.rdata.oMf[self.idR]
residualR = m2a(pinocchio.log(refMr).vector)
self.residuals = np.concatenate([residualL, residualR])
return sum(self.residuals**2)
def ik(robot,oMdes, JOINT_ID,eps = 1e-4):
q = robot.q0.copy()
IT_MAX = 500
DT = 1e-1
damp = 1e-12
i=0
success = False
while True:
pinocchio.forwardKinematics(robot.model,robot.data,q)
dMi = oMdes.actInv(robot.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(robot.model,robot.data,q,JOINT_ID)
v = - J.T.dot(solve(J.dot(J.T) + damp * np.eye(6), err))
q = pinocchio.integrate(robot.model,q,v*DT)
i += 1
if not success:
#raise( Exception('ik did not converged') )
q[:] = np.nan
if ((q < robot.model.lowerPositionLimit).any() or
(q > robot.model.upperPositionLimit).any()):
#Violate joint limits
q[:] = np.nan
return (q)
def errorInSE3( M,Mdes):
'''
Compute a 6-dim error vector (6x1 np.maptrix) caracterizing the difference
between M and Mdes, both element of SE3.
'''
error = se3.log(Mdes.inverse()*M)
return error.vector()
def cost(self, x):
q = a2m(x)
pinocchio.forwardKinematics(self.rmodel, self.rdata, q)
pinocchio.updateFramePlacements(self.rmodel, self.rdata)
M = self.rdata.oMf[self.idEff]
self.residuals = m2a(pinocchio.log(M.inverse() * self.ref).vector)
return sum(self.residuals**2)
def compute(self, x_des, M_des, q0):
self.frame_id = self.model.getFrameId('tool0')
q_i = q0
H_des = pin.SE3(M_des, x_des)
for i in range(self.n):
self.robot.computeAllTerms(q_i, np.zeros(6))
H = self.robot.framePlacement(q_i, self.frame_id, False)
dH = H_des.actInv(H)
error = pin.log(dH).vector
if (norm(error) < self.eps):
self.success = True
break
# J = self.robot.frameJacobian(q_i, self.frame_id, False)
J = pin.computeJointJacobian(self.model, self.data, q_i, 6)
v = -J.T.dot(solve(J.dot(J.T) + self.damp * np.eye(6), error))
q_i = pin.integrate(self.model, q_i, v * self.DT)
return q_i
def errorInSE3(M, Mdes):
'''
Compute a 6-dim error vector (6x1 np.maptrix) caracterizing the difference
between M and Mdes, both element of SE3.
'''
error = se3.log(Mdes.inverse()*M)
return error
def constraint(self, x):
q = a2m(x)
pinocchio.forwardKinematics(self.rmodel, self.rdata, q)
pinocchio.updateFramePlacements(self.rmodel, self.rdata)
refMeff = self.refEff.inverse() * self.rdata.oMf[self.idEff]
self.eq = m2a(pinocchio.log(refMeff).vector)
return self.eq.tolist()
def constraint_leftfoot(self, x, nc=0):
q, tauq, fr, fl = self.x2qf(x)
pinocchio.forwardKinematics(self.rmodel, self.rdata, q)
pinocchio.updateFramePlacements(self.rmodel, self.rdata)
refMl = self.refL.inverse() * self.rdata.oMf[self.idL]
self.eq[nc:nc + 6] = m2a(pinocchio.log(refMl).vector)
return self.eq[nc:nc + 6].tolist()
def constraint_rightfoot(self, x, nc=0):
q = self.vq2q(a2m(x))
pinocchio.forwardKinematics(self.rmodel, self.rdata, q)
pinocchio.updateFramePlacements(self.rmodel, self.rdata)
refMr = self.refR.inverse() * self.rdata.oMf[self.idR]
self.eq[nc:nc + 6] = m2a(pinocchio.log(refMr).vector)
return self.eq[nc:nc + 6].tolist()
def solve(self, M_des, robot, joint_id, q=None):
""" Inverse kinematics for specified joint (joint_id) and desired pose M_des (array 4x4)
NOTE The code below is adopted from here (01.03.2020):
https://gepettoweb.laas.fr/doc/stack-of-tasks/pinocchio/master/doxygen-html/md_doc_b-examples_i-inverse-kinematics.html
"""
oMdes = pinocchio.SE3(M_des[:3, :3], M_des[:3, 3])
if np.all(q == None): q = pinocchio.neutral(robot.model)
i = 0
iter_resample = 0
while True:
# forward
pinocchio.forwardKinematics(robot.model, robot.data, q)
# error between desired pose and the current one
dMi = oMdes.actInv(robot.data.oMi[joint_id])
err = pinocchio.log(dMi).vector
# Termination criteria
if norm(err) < self.inv_kin_sol_params.eps:
success = True
break
if i >= self.inv_kin_sol_params.max_iter:
if iter_resample <= self.inv_kin_sol_params.max_resample:
q = pinocchio.randomConfiguration(robot.model)
iter_resample += 1
continue
else:
success = False
break
# Jacobian
J = pinocchio.computeJointJacobian(robot.model, robot.data, q,
joint_id)
# P controller (?)
# v* =~ A * e
v = -J.T.dot(
solve(
J.dot(J.T) + self.inv_kin_sol_params.damp * np.eye(6),
err))
# integrate (in this case only sum)
q = pinocchio.integrate(robot.model, q,
v * self.inv_kin_sol_params.dt)
i += 1
if not success: q = pinocchio.neutral(robot.model)
q_arr = np.squeeze(np.array(q))
q_arr = np.mod(np.abs(q_arr), 2 * np.pi) * np.sign(q_arr)
mask = np.abs(q_arr) > np.pi
# If angle abs(q) is larger than pi, represent angle as the shorter angle inv_sign(q) * (2*pi - abs(q))
# This is to avoid conflicting with angle limits.
q_arr[mask] = (-1) * np.sign(
q_arr[mask]) * (2 * np.pi - np.abs(q_arr[mask]))
return success, q_arr
def eval_vel(delta_t):
if dofs == "TRANSLATION":
#print("delta_t:" , delta_t)
return (goal - transformation_func()) / delta_t
elif dofs is None:
return pinocchio.log(
transformation_func().inverse() * goal).vector / delta_t
else:
raise ValueError("Implementation for %s not available" % dofs)
def cost(self, x):
"""
x --> initial configuration of the robot object
"""
q = a2m(x)
pinocchio.forwardKinematics(self.rmodel, self.rdata, q)
M = self.rdata.oMi[self.idEFF]
# This is really the only difference between fKinematics_arm and this script.
self.residuals = m2a(pinocchio.log(M.inverse() * self.ref).vector)
return sum(self.residuals**2)
def inverse_kinematics(self, x, y, z):
"""Calculate the joint values for a desired cartesian position"""
model, collision_model, visual_model = pinocchio.buildModelsFromUrdf(
"/home/gabriele/catkin_ws/src/abel_move/scripts/abel_arms_full.urdf"
)
data, collision_data, visual_data = pinocchio.createDatas(
model, collision_model, visual_model)
JOINT_ID = 5
oMdes = pinocchio.SE3(np.eye(3), np.array([x, y, x]))
q = np.array(abel.sort_joints()[5:])
eps = 1e-1
IT_MAX = 1000
DT = 1e-4
damp = 1e-4
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)
point = JointTrajectoryPoint()
point.positions = [
0, 0, 0, 0, 0, q[0], q[1], q[2], q[3], q[4], q[5], q[6], q[7],
q[8], q[9]
]
abel.move_all_joints(point)
self.direct_kinematics()
def optimize_initial_position(self, init_state):
# Optimize the initial configuration
q = se3.neutral(self.robot.model)
plan_joint_init_pos = self.planner_setting.get(
PlannerVectorParam_KinematicDefaultJointPositions)
if len(plan_joint_init_pos) != self.robot.num_ctrl_joints:
raise ValueError(
'Number of joints in config file not same as required for robot\n'
+ 'Got %d joints but robot expects %d joints.' %
(len(plan_joint_init_pos), self.robot.num_ctrl_joints))
q[7:] = np.matrix(plan_joint_init_pos).T
q[2] = self.robot.floor_height + 0.32 #was 0.32
#print q[2]
dq = np.matrix(np.zeros(self.robot.robot.nv)).T
com_ref = init_state.com
lmom_ref = np.zeros(3)
amom_ref = np.zeros(3)
endeff_pos_ref = np.array(
[init_state.effPosition(i) for i in range(init_state.effNum())])
endeff_vel_ref = np.matrix(np.zeros((init_state.effNum(), 3)))
endeff_contact = np.ones(init_state.effNum())
quad_goal = se3.Quaternion(
se3.rpy.rpyToMatrix(np.matrix([0.0, 0, 0.]).T))
q[3:7] = quad_goal.coeffs()
for iters in range(self.max_iterations):
# Adding small P controller for the base orientation to always start with flat
# oriented base.
quad_q = se3.Quaternion(float(q[6]), float(q[3]), float(q[4]),
float(q[5]))
amom_ref = 1e-1 * se3.log((quad_goal * quad_q.inverse()).matrix())
res = self.inv_kin.compute(q, dq, com_ref, lmom_ref, amom_ref,
endeff_pos_ref, endeff_vel_ref,
endeff_contact, None)
q = se3.integrate(self.robot.model, q, res)
if np.linalg.norm(res) < 1e-3:
print('Found initial configuration after {} iterations'.format(
iters + 1))
break
if iters == self.max_iterations - 1:
print('Failed to converge for initial setup.')
print("initial configuration: \n", q)
q[2] = 0.20
self.q_init = q.copy()
self.dq_init = dq.copy()
def c_control(dt, robot, sample, t_simu):
eigenpy.switchToNumpyMatrix()
qa = robot.q # actuation joint position
qa_dot = robot.v # actuation joint velocity
# Method : Joint Control A q_ddot = torque
Kp = 400. # Convergence gain
ID_EE = robot.model.getFrameId(
"LWrMot3") # Control target (End effector) ID
# Compute/update all joints and frames
se3.computeAllTerms(robot.model, robot.data, qa, qa_dot)
# Get kinematics information
oMi = robot.framePlacement(qa, ID_EE) ## EE's placement
v_frame = robot.frameVelocity(qa, qa_dot, ID_EE) # EE's velocity
J = robot.computeFrameJacobian(qa,
ID_EE) ## EE's Jacobian w.r.t Local Frame
# Get dynamics information
M = robot.mass(qa) # Mass Matrix
NLE = robot.nle(qa, qa_dot) #gravity and Coriolis
Lam = np.linalg.inv(J * np.linalg.inv(M) * J.transpose()) # Lambda Matrix
# Update Target oMi position
wMl = copy.deepcopy(sample.pos)
wMl.rotation = copy.deepcopy(oMi.rotation)
v_frame_ref = se3.Motion() # target velocity (v, w)
v_frame_ref.setZero()
# Get placement Error
p_error = se3.log(sample.pos.inverse() * oMi)
v_error = v_frame - wMl.actInv(v_frame_ref)
# Task Force
p_vec = p_error.vector
v_vec = v_error.vector
for i in range(0, 3): # for position only control
p_vec[i + 3] = 0.0
v_vec[i + 3] = 0.0
f_star = Lam * (-Kp * p_vec - 2.0 * np.sqrt(Kp) * v_vec)
# Torque from Joint error
torque = J.transpose() * f_star + NLE
torque[0] = 0.0
return torque
def compute(self, time, q, v):
oMi = self.robot.framePlacement(q, self.id)
v_frame = self.robot.frameVelocity(q, v, self.id)
self.drift = self.robot.frameClassicAcceleration(self.id)
self.J = self.robot.getFrameJacobian(self.id)
self.p_error = se3.log(self.M_ref.inverse() * oMi)
self.p_error_vec = self.p_error.vector
self.p_ref = np.hstack(
(self.M_ref.translation, self.M_ref.rotation.flatten()))
self.p = np.hstack((oMi.translation, oMi.rotation.flatten()))
self.wMl.setIdentity()
self.wMl.rotation = oMi.rotation
if self.local:
self.v_error = v_frame - self.wMl.actInv(self.v_ref)
self.a_des = -self.Kp * self.p_error_vec - self.Kd * self.v_error.vector + self.wMl.actInv(
self.a_ref).vector
else:
self.p_error_vec = np.dot(self.wMl.toActionMatrix(),
self.p_error.vector)
self.v_error = self.wMl.act(v_frame) - self.v_ref
self.drift = self.wMl.act(self.drift)
self.a_des = -self.Kp * self.p_error_vec - self.Kd * self.v_error.vector + self.a_ref.vector
self.J_rotated = np.dot(self.wMl.toActionMatrix(), self.J)
self.J = self.J_rotated
self.v_error_vec = self.v_error.vector
self.v_ref_vec = self.v_ref.vector
self.v = v_frame.vector
idx = 0
for i in range(6):
if np.abs(self.mask[i] - 1.0) < 1e-5:
self.constraint.getMatrix()[idx, :] = self.J[i, :]
self.constraint.getVector()[idx] = (self.a_des -
self.drift.vector)[i]
self.a_des_masked[i] = self.a_des[i]
self.drift_masked[i] = self.drift.vector[i]
self.p_error_masked_vec[i] = self.p_error_vec[i]
self.v_error_masked_vec[i] = self.v_error_vec[i]
idx += 1
return self.constraint
def calc(self, data, x):
# We suppose forwardKinematics(q,v,a), computeJointJacobian and updateFramePlacement already
# computed.
assert (self.ref is not None or self.gains[0] == 0.)
data.J[:, :] = pinocchio.getFrameJacobian(
self.pinocchio, data.pinocchio, self.frame,
pinocchio.ReferenceFrame.LOCAL)
data.a0[:] = pinocchio.getFrameAcceleration(self.pinocchio,
data.pinocchio,
self.frame).vector.flat
if self.gains[0] != 0.:
data.rMf = self.ref.inverse() * data.pinocchio.oMf[self.frame]
data.a0[:] += self.gains[0] * m2a(pinocchio.log(data.rMf).vector)
if self.gains[1] != 0.:
data.a0[:] += self.gains[1] * m2a(
pinocchio.getFrameVelocity(self.pinocchio, data.pinocchio,
self.frame).vector)
def logarithmicMap(self, quat_wxyz):
w, x, y, z = quat_wxyz
return pin.log(pin.Quaternion(w, x, y, z).matrix())
for i in range(N):
M = se3.exp(nu)*M
viewer.updateElementConfig('RomeoTrunkYaw', se3ToRpy(M))
time.sleep(1)
# Integrate a velocity of the body that is constant in the world frame.
for i in range(N):
Mc = se3.SE3(M.rotation,zero(3))
M = M*se3.exp(Mc.actInv(nu))
viewer.updateElementConfig('RomeoTrunkYaw', se3ToRpy(M))
time.sleep(1)
# Integrate a constant "log" velocity in body frame.
ME = se3.SE3( se3.Quaternion(0.7, -0.6, 0.1, 0.4).normalized().matrix(),
np.matrix([1,-1,2],np.double).T )
nu = se3.Motion(se3.log(M.inverse()*ME).vector()/N)
for i in range(N):
M = M*se3.exp(nu)
viewer.updateElementConfig('RomeoTrunkYaw', se3ToRpy(M))
print "Residuals = ", norm( se3.log(M.inverse()*ME).vector() )
time.sleep(1)
# Integrate a constant "log" velocity in reference frame.
ME = se3.SE3( se3.Quaternion(0.3, -0.2, 0.6, 0.5).normalized().matrix(),
np.matrix([-1,1,0.6],np.double).T )
nu = se3.Motion(se3.log(M.inverse()*ME).vector()/N)
for i in range(N):
M = M*se3.exp(nu)
viewer.updateElementConfig('RomeoTrunkYaw', se3ToRpy(M))
print "Residuals = ", norm( se3.log(M.inverse()*ME).vector() )
time.sleep(1)
model, collision_model, visual_model)
JOINT_ID = 5
oMdes = pinocchio.SE3(np.eye(3), np.array([-0.19, 0.50, -0.38]))
q = pinocchio.neutral(model)
eps = 1e-2
IT_MAX = 1000
DT = 1e-4
damp = 1e-12
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!")
def cost(q):
'''Compute score from a configuration'''
q = np.matrix(q).T
M = robot.placement(q, 6)
return np.linalg.norm(pio.log(M.inverse() * Mtarget).vector)
def c_control(dt, robot, sample, t_simu):
eigenpy.switchToNumpyMatrix()
qa = np.matrix(np.zeros((7, 1)))
qa_dot = np.matrix(np.zeros((7, 1)))
for i in range(7):
qa[i, 0] = robot.q[i]
qa_dot[i, 0] = robot.v[i]
# Method : Joint Control A q_ddot = torque
Kp = 400. # Convergence gain
ID_EE = robot.model.getFrameId(
"LWrMot3") # Control target (End effector) ID
# Compute/update all joints and frames
se3.computeAllTerms(robot.model, robot.data, qa, qa_dot)
# Get kinematics information
oMi = robot.framePlacement(qa, ID_EE) ## EE's placement
v_frame = robot.frameVelocity(qa, qa_dot, ID_EE) # EE's velocity
J = robot.computeFrameJacobian(
qa, ID_EE)[:3, :] ## EE's Jacobian w.r.t Local Frame
# Get dynamics information
M = robot.mass(qa) # Mass Matrix
NLE = robot.nle(qa, qa_dot) #gravity and Coriolis
Lam = np.linalg.inv(J * np.linalg.inv(M) * J.transpose()) # Lambda Matrix
# Update Target oMi position
wMl = copy.deepcopy(sample.pos)
wMl.rotation = copy.deepcopy(oMi.rotation)
v_frame_ref = se3.Motion() # target velocity (v, w)
v_frame_ref.setZero()
# Get placement Error
p_error = se3.log(sample.pos.inverse() * oMi)
v_error = v_frame - wMl.actInv(v_frame_ref)
# Task Force
p_vec = p_error.linear
v_vec = v_error.linear
xddotstar = (-Kp * p_vec - 2 * np.sqrt(Kp) * v_vec)
f_star = Lam * xddotstar
# Torque from Joint error
torque = J.transpose() * f_star + NLE
torque[0] = 0.0
# Selection Matrix
Id7 = np.identity(7)
fault_joint_num = 0
S = np.delete(Id7, (fault_joint_num),
axis=0) #delete fault joint corresponding row
# Selection Matrix Inverse - dynamically consistent inverse
Minv = np.linalg.inv(M)
ST = S.transpose()
SbarT = np.linalg.pinv(S * Minv * ST) * S * Minv
# Jacobian Matrix Inverse - dynamically consistent inverse
JbarT = Lam * J * np.linalg.inv(M)
#Weighting matrix
W = S * Minv * ST
Winv = np.linalg.inv(W)
#Weighted pseudo inverse of JbarT*ST
JtildeT = Winv * (JbarT * ST).transpose() * np.linalg.pinv(
JbarT * ST * Winv * (JbarT * ST).transpose())
#Null-space projection matrix
Id6 = np.identity(6)
NtildeT = Id6 - JtildeT * JbarT * ST
null_torque = 0.0 * qa_dot # joint damping
#Torque
torque = ST * (JtildeT * f_star + NtildeT * SbarT * null_torque +
SbarT * NLE)
return torque
def residual(self, q):
'''Compute score from a configuration'''
pin.forwardKinematics(self.rmodel, self.rdata, q)
M = pin.updateFramePlacement(self.rmodel, self.rdata, self.frameIndex)
self.deltaM = self.Mtarget.inverse() * M
return pin.log(self.deltaM).vector
M = se3.exp(nu) * M
viewer.updateElementConfig('RomeoTrunkYaw', se3ToRpy(M))
time.sleep(1)
# Integrate a velocity of the body that is constant in the world frame.
for i in range(N):
Mc = se3.SE3(M.rotation, zero(3))
M = M * se3.exp(Mc.actInv(nu))
viewer.updateElementConfig('RomeoTrunkYaw', se3ToRpy(M))
time.sleep(1)
# Integrate a constant "log" velocity in body frame.
ME = se3.SE3(
se3.Quaternion(0.7, -0.6, 0.1, 0.4).normalized().matrix(),
np.matrix([1, -1, 2], np.double).T)
nu = se3.Motion(se3.log(M.inverse() * ME).vector() / N)
for i in range(N):
M = M * se3.exp(nu)
viewer.updateElementConfig('RomeoTrunkYaw', se3ToRpy(M))
print "Residuals = ", norm(se3.log(M.inverse() * ME).vector())
time.sleep(1)
# Integrate a constant "log" velocity in reference frame.
ME = se3.SE3(
se3.Quaternion(0.3, -0.2, 0.6, 0.5).normalized().matrix(),
np.matrix([-1, 1, 0.6], np.double).T)
nu = se3.Motion(se3.log(M.inverse() * ME).vector() / N)
for i in range(N):
M = M * se3.exp(nu)
viewer.updateElementConfig('RomeoTrunkYaw', se3ToRpy(M))
print "Residuals = ", norm(se3.log(M.inverse() * ME).vector())
inv = np.linalg.inv
#WBIC from https://arxiv.org/pdf/1909.06586.pdf
M = robot.mass(q)
#base orientation (3d)
Kp1 = Kp_base_orientation
Kd1 = Kd_base_orientation
J1 = J_base_orientation
M1 = robot.framePlacement(q, BASE_ID).rotation
dx1 = robot.frameVelocity(q, dq, BASE_ID).angular
M1_des = base_orientation_ref
dx1_des = base_angularvelocity_ref
ddx1_des = base_angularacceleration_ref
e1 = pin.log(M1_des.T * M1)
ddx1_cmd = ddx1_des + Kp1 * e1 + Kd1 * (dx1_des - dx1)
dJ1dq = 0 #robot.frameAcceleration(q,dq,0*dq,BASE_ID).angular # TODO check this !
#base position (3d)
Kp2 = Kp_base_position
Kd2 = Kd_base_position
J2 = J_base_position
x2 = robot.framePlacement(q, BASE_ID).translation
dx2 = robot.frameVelocity(q, dq, BASE_ID).linear
x2_des = base_position_ref
dx2_des = base_linearvelocity_ref
ddx2_des = base_linearacceleration_ref
e2 = x2_des - x2
ddx2_cmd = ddx2_des + Kp2 * e2 + Kd2 * (dx2_des - dx2)
dJ2dq = 0 #robot.frameAcceleration(q,dq,0*dq,BASE_ID).linear # TODO check this !
def errorInSE3(M, Mdes):
error = se3.log(Mdes.inverse() * M)
return error
def optimize(self,
init_state,
contact_sequence,
dynamic_sequence,
plotting=False):
self.init_state = init_state
self.contact_sequence = contact_sequence
self.dynamic_sequence = dynamic_sequence
self.q_via = None
# Create array with centroidal and endeffector informations.
self.fill_data_from_dynamics()
self.fill_endeffector_trajectory()
# Run the optimization for the initial configuration only once.
if self.q_init is None:
self.optimize_initial_position(init_state)
# Get the desired joint trajectory
# print "num_joint_via:",self.planner_setting.get(PlannerIntParam_NumJointViapoints)
# print "joint_via:",self.planner_setting.get(PlannerCVectorParam_JointViapoints)
# TODO: this is for jump, should go to config file
# q_jump = [1., 0.1, -0.2 ,0.1, -0.2 ,-0.1, 0.2 ,-0.1, 0.2]
# q_via = np.matrix([.75, np.pi/2, -np.pi, np.pi/2, -np.pi, -np.pi/2, np.pi, -np.pi/2, np.pi]).T
# q_max = np.matrix([1.35, .7*np.pi/2, -.7*np.pi, .7*np.pi/2, -.7*np.pi, -.7*np.pi/2, .7*np.pi, -.7*np.pi/2, .7*np.pi]).T
# q_via0 = np.vstack((q_via.T, q_jump))
# self.q_via = np.vstack((q_via0, q_max.T))
joint_traj_gen = JointTrajectoryGenerator()
joint_traj_gen.num_time_steps = self.num_time_steps
joint_traj_gen.q_init = self.q_init[7:]
self.joint_des = np.zeros((len(self.q_init[7:]), self.num_time_steps),
float)
if self.q_via is None:
for i in range(self.num_time_steps):
self.joint_des[:, i] = self.q_init[7:].T
else:
joint_traj_gen.joint_traj(self.q_via)
for it in range(self.num_time_steps):
self.joint_des[:, it] = joint_traj_gen.eval_traj(it)
# Compute inverse kinematics over the full trajectory.
self.inv_kin.is_init_time = 0
q, dq = self.q_init.copy(), self.dq_init.copy()
for it in range(self.num_time_steps):
quad_goal = se3.Quaternion(
se3.rpy.rpyToMatrix(np.matrix([0.0, 0, 0.]).T))
quad_q = se3.Quaternion(float(q[6]), float(q[3]), float(q[4]),
float(q[5]))
amom_ref = (self.reg_orientation * se3.log(
(quad_goal * quad_q.inverse()).matrix()).T +
self.amom_dyn[it]).reshape(-1)
joint_regularization_ref = self.reg_joint_position * (
np.matrix(self.joint_des[:, it]).T - q[7:])
# joint_regularization_ref = self.reg_joint_position * (self.q_init[7 : ] - q[7 : ])
# Fill the kinematics results for it.
self.inv_kin.forward_robot(q, dq)
self.fill_kinematic_result(it, q, dq)
dq = self.inv_kin.compute(q, dq, self.com_dyn[it],
self.lmom_dyn[it], amom_ref,
self.endeff_pos_ref[it],
self.endeff_vel_ref[it],
self.endeff_contact[it],
joint_regularization_ref)
# Integrate to the next state.
q = se3.integrate(self.robot.model, q, dq * self.dt)
def calc(self, data, x, u):
data.rMf = self.Mref.oMf.inverse() * data.pinocchio.oMf[
self.Mref.frame]
data.r = pinocchio.log(data.rMf).vector
self.activation.calc(data.activation, data.r)
data.cost = data.activation.a
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,
q) # Compute frame placements
oMtool = robot.data.oMf[
IDX_TOOL] # Placement from world frame o to frame f oMtool
tool_nu = pio.log(oMtool.inverse() *
oMgoal).vector # 6D error between the two frame
tool_Jtool = pio.computeFrameJacobian(
robot.model, robot.data, q, IDX_TOOL) # Get corresponding jacobian
vq = pinv(tool_Jtool) * tool_nu
q = pio.integrate(robot.model, q, vq * DT)
robot.display(q)
time.sleep(1e-3)
herr.append(tool_nu)
def residualrMi(q):
pinocchio.forwardKinematics(rmodel, rdata, q)
return pinocchio.log(Mref.inverse() * rdata.oMi[jid]).vector
def optimize(self,
init_state,
contact_sequence,
dynamic_sequence,
plotting=False):
self.init_state = init_state
self.contact_sequence = contact_sequence
self.dynamic_sequence = dynamic_sequence
# Create array with centroidal and endeffector informations.
self.fill_data_from_dynamics()
self.fill_endeffector_trajectory()
# Run the optimization for the initial configuration only once.
if self.q_init is None:
self.optimize_initial_position(init_state)
# Get the desired joint trajectory
n_via_joint = self.planner_setting.get(
PlannerIntParam_NumJointViapoints)
via_joint = self.planner_setting.get(
PlannerCVectorParam_JointViapoints)
self.joint_des = np.zeros((len(self.q_init[7:]), self.num_time_steps),
float)
if n_via_joint == 0:
for i in range(self.num_time_steps):
self.joint_des[:, i] = self.q_init[7:].T
else:
joint_traj_gen = TrajectoryInterpolator()
joint_traj_gen.num_time_steps = self.num_time_steps
joint_traj_gen.init = self.q_init[7:]
joint_traj_gen.end = self.q_init[7:]
joint_traj_gen.generate_trajectory(n_via_joint, via_joint, self.dt)
for it in range(self.num_time_steps):
self.joint_des[:, it] = joint_traj_gen.evaluate_trajecory(it)
# Get the desired base viapoints
n_via_base = self.planner_setting.get(PlannerIntParam_NumBaseViapoints)
via_base = self.planner_setting.get(PlannerCVectorParam_BaseViapoints)
# Generate smooth base trajectory for regularization
self.base_des = np.zeros((3, self.num_time_steps), float)
if n_via_base == 0:
for it in range(self.num_time_steps):
self.base_des[:, it] = np.array([0., 0., 0.]).reshape(-1)
else:
base_traj_gen = TrajectoryInterpolator()
base_traj_gen.num_time_steps = self.num_time_steps
base_traj_gen.init = np.array([0.0, 0.0, 0.0])
base_traj_gen.end = np.array([0.0, 0.0, 0.0])
base_traj_gen.generate_trajectory(n_via_base, via_base, self.dt)
for it in range(self.num_time_steps):
self.base_des[:, it] = base_traj_gen.evaluate_trajecory(it)
# Compute inverse kinematics over the full trajectory.
self.inv_kin.is_init_time = 0
q, dq = self.q_init.copy(), self.dq_init.copy()
for it in range(self.num_time_steps):
quad_goal = se3.Quaternion(
se3.rpy.rpyToMatrix(np.matrix(self.base_des[:, it]).T))
quad_q = se3.Quaternion(float(q[6]), float(q[3]), float(q[4]),
float(q[5]))
amom_ref = self.reg_orientation * se3.log(
(quad_goal * quad_q.inverse()).matrix()) + self.amom_dyn[it]
joint_regularization_ref = self.reg_joint_position * (
self.joint_des[:, it] - q[7:])
# Fill the kinematics results for it.
self.inv_kin.forward_robot(q, dq)
self.fill_kinematic_result(it, q, dq)
dq = self.inv_kin.compute(q, dq, self.com_dyn[it],
self.lmom_dyn[it], amom_ref,
self.endeff_pos_ref[it],
self.endeff_vel_ref[it],
self.endeff_contact[it],
joint_regularization_ref)
# Integrate to the next state.
q = se3.integrate(self.robot.model, q, dq * self.dt)
def calc(self, data, x, u):
data.rMf = self.ref.inverse() * data.pinocchio.oMf[self.frame]
data.residuals[:] = m2a(pinocchio.log(data.rMf).vector)
data.cost = sum(self.activation.calc(data.activation, data.residuals))
return data.cost
N = 1000
v = zero(3); v[2] = 1.0 / N
w = zero(3); w[1] = 1.0 / N
nu = se3.Motion( v, w )
M = se3.SE3.Identity()
updateJointConfiguration(M,'HeadRollLink')
for i in range(N):
M = M*se3.exp(nu)
updateJointConfiguration(M,'HeadRollLink')
N = 1000
Mdes = se3.SE3.Random()
nu = se3.log(M.inverse()*Mdes)
for i in range(N):
M = M*se3.exp(nu.vector()/N)
updateJointConfiguration(M,'HeadRollLink')
def errorInSE3( M,Mdes):
'''
Compute a 6-dim error vector (6x1 np.maptrix) caracterizing the difference
between M and Mdes, both element of SE3.
'''
error = se3.log(M.inverse()*Mdes)
return error.vector()
N = 1000
Mdes = se3.SE3.Random()