-
Notifications
You must be signed in to change notification settings - Fork 0
/
panda_kinematics.py
44 lines (31 loc) · 1.48 KB
/
panda_kinematics.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
from abc import ABC
from abc import abstractmethod
import math
import numpy as np
import panda_ik_pump
import panda_ik_hand
class PandaKinematics(ABC):
@abstractmethod
def solver(self, initial_joint_positions, position, orientation_quat, q7):
pass
def ik(self, initial_joint_positions, position, orientation_quat):
angles = np.linspace(-2.89, 2.89, 50)
for angle in angles:
q7 = angle
initial_joint_positions[-1] = q7
solutions = self.solver(position, orientation_quat, q7, initial_joint_positions)
for solution in solutions:
if all([not math.isnan(solution[i]) for i in range(7)]):
return solution
class PandaWithHandKinematics(PandaKinematics):
def solver(self, position, orientation_quat, q7, initial_joint_positions):
return panda_ik_hand.franka_IK(position, orientation_quat, q7, initial_joint_positions)
class PandaWithPumpKinematics(PandaKinematics):
def solver(self, position, orientation_quat, q7, initial_joint_positions):
return panda_ik_pump.franka_IK(position, orientation_quat, q7, initial_joint_positions)
def ik(self, initial_joint_positions, position, orientation_quat):
solution = super().ik(initial_joint_positions, position, orientation_quat)
if solution is not None:
# When not using the hand, we need to remove 45 degrees from the last joint.
solution[-1] -= 0.785398
return solution