def update_input(self):
m2mm = np.matrix(np.diag(np.array([1000, 1000, 1000, 1, 1, 1])))
mm2m = np.matrix(np.diag(np.array([0.001, 0.001, 0.001, 1, 1, 1])))
jacobi = get_jacobi_matrix(self._robot_state_cur.arm_joint_angles,
euler_from_rosquaternion(self._robot_state_cur.base_pose.orientation)[0])
# pinv_jacobi = np.linalg.pinv(jacobi)
pinv_jacobi = jacobi.T.dot(np.linalg.inv(jacobi.dot(jacobi.T)))
# vec_redundancy = get_vec_redundancy(self._robot_state_cur, jacobi)
Kpgain = np.matrix(np.diag(np.array([2, 2, 2, 0, 0, 0])))
Kredundancy = np.matrix(np.diag(np.array([1, 1, 1, 1, 1, 1, 1, 1])))
# rospy.loginfo('\n' + '\n'.join(['EEStateErr:\t' + ', '.join([str(d[0]) for d in self._ee_state_err.tolist()]),
# 'EEVelRef:\t' + ', '.join([str(d[0]) for d in self._ee_vel_ref.tolist()])]))
u = pinv_jacobi * m2mm * (self._ee_vel_ref - Kpgain * self._ee_state_err)
# + (np.identity(8) - pinv_jacobi * jacobi) * Kredundancy * vec_redundancy
u = u.round(decimals=self._decimals)
v = (u[6, 0] + u[7, 0]) * self._param.Rw / 2
w = (u[6, 0] - u[7, 0]) * self._param.Rw / self._param.T
self._ee_state_err_container.write(self._ee_state_err.T.tolist()[0])
self._ee_state_ref_container.write(self._ee_state_ref.T.tolist()[0])
self._ee_state_cur_container.write(self._ee_state_cur.T.tolist()[0])
self._ee_state_cur_container.write(self._ee_state_cur.T.tolist()[0])
self._robot_vel_input_container.write(u.T.tolist()[0][:6] + [v, w])
# FIXME u[0, 0]
return [JointVelocity(joint1=-u[0, 0], joint2=u[1, 0], joint3=u[2, 0],
joint4=u[3, 0], joint5=u[4, 0], joint6=u[5, 0]),
TwistStamped(header=Header(stamp=rospy.Time.now()),
twist=twist_from_vw(v, w))]
def update_input(self):
v, w = self._calc_pd_input()
return TwistStamped(header=Header(stamp=rospy.Time.now()), twist=twist_from_vw(v, w))
