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()