def calc_inv_pos(angles, target_pos, target_ori, epsilon, right=True):
p = np.array([0, 0, 0, 1])
angs = np.array([a for a in angles])
sum_old = 100000
while True:
pos, ori, j = fk.calc_fk_and_jacob(angs, jacob=True, right=right)
J = _calc_invJ(j)
delta_pos = np.matrix((target_pos - pos)[0:3]).transpose()
delta_ori = np.matrix((target_ori - ori)[0:3]).transpose()
delta = np.vstack((delta_pos, delta_ori))
v_w = (J * delta).transpose()
angs = np.squeeze(np.asarray(v_w)) + angs
sum = 0
for d in delta:
sum = sum + math.fabs(d)
#sum = np.sum(delta_pos)
if sum < epsilon:
break
if sum > sum_old:
print '# set_position error : Distance can not converged.'
return None
sum_old = sum
return angs
def left_arm_get_position(angles):
"""
Just calculate the position when joints on the pepper's left arm is in given positions
Args:
angles : Angles of left arm joints (list of 5 double values. unit is radian)
Returns:
A tuple of two arrays (position, orientation). orientation is presented as Matrix. Unit = meter.
(position, orientation) = (np.array([position_x, position_y, position_z]), np.array([[R00, R01, R02], [R10, R11, R12], [R20, R21, R22]]))
"""
return fk.calc_fk_and_jacob(angles, jacob=False, right=False)
def left_arm_get_position(angles):
"""
Just calculate the position when joints on the pepper's left arm is in given positions
Args:
angles : Angles of left arm joints (list of 5 double values. unit is radian)
Returns:
A tuple of two arrays (position, orientation). orientation is presented as Matrix. Unit = meter.
(position, orientation) = (np.array([position_x, position_y, position_z]), np.array([[R00, R01, R02], [R10, R11, R12], [R20, R21, R22]]))
"""
return fk.calc_fk_and_jacob(angles, jacob=False, right=False)
def calc_inv_pos(angles, target_pos, target_ori, epsilon, right=True):
p = np.array([0,0,0,1])
angs = np.array([a for a in angles])
sum_old = 100000
while True:
pos, ori, j = fk.calc_fk_and_jacob(angs, jacob=True, right=right)
J = _calc_invJ(j)
delta_pos = np.matrix((target_pos-pos)[0:3]).transpose()
v = (J * delta_pos).transpose()
angs = np.squeeze(np.asarray(v)) + angs
sum = 0
for d in delta_pos:
sum = sum + math.fabs(d)
#sum = np.sum(delta_pos)
if sum < epsilon:
break
if sum > sum_old:
print '# set_position error : Distance can not converged.'
return None
sum_old = sum
return angs
