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 _calc_pd_input(self):
Kp, Kd = 1, 1
theta = euler_from_rosquaternion(self.base_pose_cur.orientation)[0]
R = np.matrix([[np.cos(theta), np.sin(theta)], [-np.sin(theta), np.cos(theta)]])
Xd_dot = np.matrix([[self.base_twist_ref.linear.x], [self.base_twist_ref.linear.y]])
Xd = np.matrix([[self.base_pose_ref.position.x], [self.base_pose_ref.position.y]])
X = np.matrix([[self.base_pose_cur.position.x], [self.base_pose_cur.position.y]])
u = R * (Kd * Xd_dot - Kp * (X - Xd))
v, w = float(u[0]), float(u[1])
return v, w
def _bs_input_callback(self, bs_input):
dt = self.timer.dt_sec
vdt = self._input.twist.linear.x * dt
wdt = self._input.twist.angular.z * dt
theta = euler_from_rosquaternion(self._mobile_base_tf_for_calc_from_localized_origin.transform.rotation)[0]
translation = add_vectors([self._mobile_base_tf_for_calc_from_localized_origin.transform.translation,
Vector3(x=vdt * np.cos(theta), y=vdt * np.sin(theta))])
rotation = multiply_ros_quaternion([self._mobile_base_tf_for_calc_from_localized_origin.transform.rotation,
Quaternion(z=np.sin(wdt / 2.0), w=np.cos(wdt / 2.0))])
self._mobile_base_tf_for_calc_from_localized_origin.header.stamp = rospy.Time.now()
self._mobile_base_tf_for_calc_from_localized_origin.transform.translation = translation
self._mobile_base_tf_for_calc_from_localized_origin.transform.rotation = rotation
self._localized_point_tf_for_calc_from_mobile_base = update_tf(self._tf_buffer,
self._localized_point_tf_for_calc_from_mobile_base)
self._input = bs_input
self.timer.update()
def yaw_bw_orientations(ori1, ori2):
return euler_from_rosquaternion(ori1)[0] - euler_from_rosquaternion(ori2)[0]