def command_velocity(self, squiggle): ''' Commands joint velocities using jacobian magicks ''' J = self._left_kin.jacobian() Jinv = hf.rpinv(J) q_dot = Jinv*squiggle + (np.identity(7) - (Jinv*J))*self.get_b(self.K0, self.DELTA) cmd = {joint: q_dot[i, 0] for i, joint in enumerate(self._left_joint_names)} self._left_arm.set_joint_velocities(cmd)
def command_velocity(self, squiggle): ''' Commands joint velocities using jacobian Squiggle is an np.array (1D)!!!!!! ''' squiggle = np.matrix(squiggle).T J = self._kin.jacobian() Jinv = hf.rpinv(J) q_dot = Jinv*squiggle + (np.identity(7) - (Jinv*J))*self.get_b(self.K0) cmd = {joint: q_dot[i, 0] for i, joint in enumerate(self._joint_names)} self._arm_obj.set_joint_velocities(cmd)
def command_velocity(self, squiggle): ''' Commands joint velocities using jacobian magicks ''' J = self._left_kin.jacobian() Jinv = hf.rpinv(J) q_dot = Jinv * squiggle + (np.identity(7) - (Jinv * J)) * self.get_b( self.K0, self.DELTA) cmd = { joint: q_dot[i, 0] for i, joint in enumerate(self._left_joint_names) } self._left_arm.set_joint_velocities(cmd)
def calc_end_velocities(self, u, v, Z): x, y = self.img_xy_from_uv(u, v) L = self.interaction_matrix(x, y, Z) s = self.calc_desired_feat_velocities(u, v) # print 'Feat vels:', s return np.array(hf.rpinv(L)*s).flatten()