def step(self, dt: float = 0.01) -> None: # pylint: disable=unused-argument """ Updates the robot joints (robot.q) used in computing kinematics :param dt: the delta time since the last update, defaults to 0.01 :type dt: float, optional """ if self.readonly: return current_time = timeit.default_timer() Kp = 1 self.publish_state() # calculate joint velocities from desired cartesian velocity if any(self.e_v): if current_time - self.last_update > 0.1: self.e_v *= 0.9 if np.sum(np.absolute(self.e_v) ) >= 0.0001 else 0 wTe = self.fkine(self.q, start=self.base_link, fast=True, end=self.gripper) error = self.e_p @ np.linalg.inv(wTe) # print(e) trans = self.e_p[:3, 3] # .astype('float64') trans += self.e_v[:3] * dt rotation = SO3(self.e_p[:3, :3]) # Rdelta = SO3.EulerVec(self.e_v[3:]) # R = Rdelta * R # R = R.norm() # # print(self.e_p) self.e_p = SE3.Rt(rotation, t=trans).A v_t = self.e_v[:3] + Kp * error[:3, 3] v_r = self.e_v[3:] # + (e[.rpy(]) * 0.5) e_v = np.concatenate([v_t, v_r]) self.j_v = np.linalg.pinv( self.jacob0(self.q, fast=True, end=self.gripper)) @ e_v # apply desired joint velocity to robot if any(self.j_v): if current_time - self.last_update > 0.1: self.j_v *= 0.9 if np.sum(np.absolute(self.j_v) ) >= 0.0001 else 0 self.qd = self.j_v self.joint_publisher.publish(Float64MultiArray(data=self.qd)) self.last_tick = current_time self.event.set()
def convert(R): return BASE * SE3.Rt(R, t=[0, 0, 0])
def test_constructor(self): # null constructor R = SE3() nt.assert_equal(len(R), 1) array_compare(R, np.eye(4)) self.assertIsInstance(R, SE3) # construct from matrix R = SE3(trotx(0.2)) nt.assert_equal(len(R), 1) array_compare(R, trotx(0.2)) self.assertIsInstance(R, SE3) # construct from canonic rotation R = SE3.Rx(0.2) nt.assert_equal(len(R), 1) array_compare(R, trotx(0.2)) self.assertIsInstance(R, SE3) R = SE3.Ry(0.2) nt.assert_equal(len(R), 1) array_compare(R, troty(0.2)) self.assertIsInstance(R, SE3) R = SE3.Rz(0.2) nt.assert_equal(len(R), 1) array_compare(R, trotz(0.2)) self.assertIsInstance(R, SE3) # construct from canonic translation R = SE3.Tx(0.2) nt.assert_equal(len(R), 1) array_compare(R, transl(0.2, 0, 0)) self.assertIsInstance(R, SE3) R = SE3.Ty(0.2) nt.assert_equal(len(R), 1) array_compare(R, transl(0, 0.2, 0)) self.assertIsInstance(R, SE3) R = SE3.Tz(0.2) nt.assert_equal(len(R), 1) array_compare(R, transl(0, 0, 0.2)) self.assertIsInstance(R, SE3) # triple angle R = SE3.Eul([0.1, 0.2, 0.3]) nt.assert_equal(len(R), 1) array_compare(R, eul2tr([0.1, 0.2, 0.3])) self.assertIsInstance(R, SE3) R = SE3.Eul(np.r_[0.1, 0.2, 0.3]) nt.assert_equal(len(R), 1) array_compare(R, eul2tr([0.1, 0.2, 0.3])) self.assertIsInstance(R, SE3) R = SE3.Eul([10, 20, 30], unit='deg') nt.assert_equal(len(R), 1) array_compare(R, eul2tr([10, 20, 30], unit='deg')) self.assertIsInstance(R, SE3) R = SE3.RPY([0.1, 0.2, 0.3]) nt.assert_equal(len(R), 1) array_compare(R, rpy2tr([0.1, 0.2, 0.3])) self.assertIsInstance(R, SE3) R = SE3.RPY(np.r_[0.1, 0.2, 0.3]) nt.assert_equal(len(R), 1) array_compare(R, rpy2tr([0.1, 0.2, 0.3])) self.assertIsInstance(R, SE3) R = SE3.RPY([10, 20, 30], unit='deg') nt.assert_equal(len(R), 1) array_compare(R, rpy2tr([10, 20, 30], unit='deg')) self.assertIsInstance(R, SE3) R = SE3.RPY([0.1, 0.2, 0.3], order='xyz') nt.assert_equal(len(R), 1) array_compare(R, rpy2tr([0.1, 0.2, 0.3], order='xyz')) self.assertIsInstance(R, SE3) # angvec R = SE3.AngVec(0.2, [1, 0, 0]) nt.assert_equal(len(R), 1) array_compare(R, trotx(0.2)) self.assertIsInstance(R, SE3) R = SE3.AngVec(0.3, [0, 1, 0]) nt.assert_equal(len(R), 1) array_compare(R, troty(0.3)) self.assertIsInstance(R, SE3) # OA R = SE3.OA([0, 1, 0], [0, 0, 1]) nt.assert_equal(len(R), 1) array_compare(R, np.eye(4)) self.assertIsInstance(R, SE3) # random R = SE3.Rand() nt.assert_equal(len(R), 1) self.assertIsInstance(R, SE3) # random T = SE3.Rand() R = T.R t = T.t T = SE3.Rt(R, t) self.assertIsInstance(T, SE3) self.assertEqual(T.A.shape, (4, 4)) nt.assert_equal(T.R, R) nt.assert_equal(T.t, t) # copy constructor R = SE3.Rx(pi / 2) R2 = SE3(R) R = SE3.Ry(pi / 2) array_compare(R2, trotx(pi / 2)) # SO3 T = SE3(SO3()) nt.assert_equal(len(T), 1) self.assertIsInstance(T, SE3) nt.assert_equal(T.A, np.eye(4)) # SE2 T = SE3(SE2(1, 2, 0.4)) nt.assert_equal(len(T), 1) self.assertIsInstance(T, SE3) self.assertEqual(T.A.shape, (4, 4)) nt.assert_equal(T.t, [1, 2, 0])