|
MAGIKS
1.1
Manipulator General Inverse Kinematic Solver
|
|
MAGIKS
1.1
Manipulator General Inverse Kinematic Solver
|
Contains properties and methods managing the kinematics of the PR2 robot arm. More...
Public Member Functions | |
| def | __init__ |
| The Class Constructor: More... | |
| def | set_config |
| Sets the robot configuration to the desired given joint values. More... | |
| def | set_target |
| Sets the endeffector target to a desired position and orientation. More... | |
| def | elbow_position |
| Use this function to get the current position of the elbow joint center. More... | |
| def | wrist_position |
| Use this function to get the current position of the wrist joint center. More... | |
| def | wrist_orientation |
| Use this function to get the current orientation of the gripper(Endeffector). More... | |
| def | in_target |
| Use this function to check if the endeffector is in the target pose or not. More... | |
| def | div_phi_err |
| protected More... | |
| def | div_theta_err |
| protected More... | |
| def | joint_redundancy_jacobian |
| Use this function to get the Joint Redundancy Jacobian vector of the arm. More... | |
| def | grown_phi |
| protected More... | |
| def | position_permission_workspace |
| Use this method to get the permission range of x, y and z of the endeffector. More... | |
| def | restore_config |
| returns the current configuration and sets the given configuration. More... | |
| def | pose_metric |
| Use this function to get the displacement between the actual and desired wrist poses The metric returns sum of the norms of position difference Nd Frobenus norm of the orientation difference. More... | |
| def | optimal_config |
| Finds the optimal value for the redundant parameter \( \phi \) that minimizes the cost function and returns the joints corresponding with optimal redundant parameter value. More... | |
| def | moveto_optimal |
| Moves the joints within the solution manifold towards optimum configuration. More... | |
| def | closest_feasible_phi |
| This function should be used when given redundant parameter \( \phi \) is within the given permission set (PS) and yet there is no IK solution for it. More... | |
| def | ik_direction |
| Solves the position based inverse kinematics and returns a direction in the jointspace towards the solution (joint correction) More... | |
| def | feasible_joint_stepsize |
| Given a direction for joint correction and a joint speed limit, this function returns the maximum feasible value by which the joints can move in this direction. More... | |
| def | move_joints_towards |
| def | moveto_target |
| def | goto_target |
| def | all_IK_solutions |
| def | IK_config |
| def | ts_project |
| def | js_project |
| def | sync_from_magiks |
| def | permission_set_position |
| def | delta_phi_interval |
Public Attributes | |
| orientation_respected | |
| A boolean specifying if orientation of the endeffector should be respected or not in finding the Inverse Kinematic solution. More... | |
| silent | |
| config | |
| An instance of class PR2_ARM_CONFIGURATION() containing the jointspace properties and methods of the arm. More... | |
| Rd | |
| A \( 3 \times 3 \) rotation matrix specifying the desired endeffector orientation for the IK problem. More... | |
| xd | |
| A numpy vector of size 3 specifying the desired endeffector position for the IK problem. More... | |
| a0 | |
| A float specifying the value of \( a_0 \) in the DH parameters of the arm. More... | |
| d2 | |
| A float specifying the value of \( d_2 \) in the DH parameters of the arm. More... | |
| d4 | |
| A float specifying the value of \( d_4 \) in the DH parameters of the arm. More... | |
| dt | |
| max_js | |
| max_ja | |
| l | |
| additional_dims | |
| protected More... | |
| l_se | |
| protected More... | |
| l_ew | |
| ofuncode | |
| An integer between 0 and 1 specifying the objective function code. More... | |
| wrist_position_vector | |
| An numpy array of size 3 containing the position of the wrist joint center w.r.t. More... | |
| in_target_flag | |
| wrist_orientation_matrix | |
| A \( 3 \times 3 \) numpy matrix representing the orientation of the endeffector (gripper) w.r.t. More... | |
| JRJ | |
| A numpy array containing the Joint Redundancy Jacobian of the arm. More... | |
| E | |
| protected More... | |
| F | |
| protected More... | |
| Phi | |
| Phi A variable of type Interval() containing the feasible interval for the redundant parameter \( \phi \) or the first joint angle (Shoulder Pan Joint) More... | |
| Delta | |
| Delta A variable of type Interval() containing the feasible interval for the growth of redundant parameter ( \( \Delta \phi \)) More... | |
| ik | |
| magiks_running | |
| use_magiks | |
| target_parameters | |
Contains properties and methods managing the kinematics of the PR2 robot arm.
This class contains all properties and methods required for forward and inverse kinematic computations and kinematic control of the arm
Definition at line 376 of file pr2_arm_kinematics.py.
| def __init__ | ( | self, | |
a0 = 0.1, |
|||
d2 = 0.4, |
|||
d4 = 0.321, |
|||
ql = default_ql, |
|||
qh = default_qh, |
|||
W = default_W, |
|||
run_magiks = False |
|||
| ) |
The Class Constructor:
| a0 | A float specifying the value of /f$ a_0 /f$ in the DH parameters of the arm |
| d2 | A float specifying the value of /f$ d_2 /f$ in the DH parameters of the arm |
| d4 | A float specifying the value of /f$ d_4 /f$ in the DH parameters of the arm |
| ql | A numpy array of size 7 containing the lower bounds of the arm joints |
| qh | A numpy array of size 7 containing the upper bounds of the arm joints |
| W | A numpy array of size 7 containing the weights of each joint in the objective function. The joint weights will be used in the optimization of redundancy.
If the weight of a joint is 0, the joint is considered as unlimited.
This means the value of that joint does not need to be in a specific range. |
Definition at line 389 of file pr2_arm_kinematics.py.
| def all_IK_solutions | ( | self, | |
| phi | |||
| ) |
Finds all the feasible solutions of the Inverse Kinematic problem for given redundant parameter "phi" "phi" is the value of the first joint angle "q[0]" This function does NOT set the configuration so the joints do not change
Definition at line 1088 of file pr2_arm_kinematics.py.
| def closest_feasible_phi | ( | self, | |
| phi, | |||
| PS, | |||
increment = math.pi/360.0 |
|||
| ) |
This function should be used when given redundant parameter \( \phi \) is within the given permission set (PS) and yet there is no IK solution for it.
The neighborhood of \( \phi \) is searched for a feasible IK solution. phi A scalar float specifying the desired value of the redundant parameter \( \phi \) (No Default value set) PS A list of variables of type interval. It can be the output of function If a solution is found, the config is set and a True is returned, If all permission set is searched with no solution, False is returned and the object configuration will not change.
Definition at line 936 of file pr2_arm_kinematics.py.
| def delta_phi_interval | ( | self, | |
respect_js = False |
|||
| ) |
Updates the feasible interval for the growth of the redundant parameter according to the specified joint limits and the current value of the joints
Definition at line 1485 of file pr2_arm_kinematics.py.
| def div_phi_err | ( | self | ) |
protected
Definition at line 641 of file pr2_arm_kinematics.py.
| def div_theta_err | ( | self | ) |
protected
Definition at line 650 of file pr2_arm_kinematics.py.
| def elbow_position | ( | self | ) |
Use this function to get the current position of the elbow joint center.
| None |
Definition at line 561 of file pr2_arm_kinematics.py.
| def feasible_joint_stepsize | ( | self, | |
| direction, | |||
| max_speed, | |||
time_step = 0.1 |
|||
| ) |
Given a direction for joint correction and a joint speed limit, this function returns the maximum feasible value by which the joints can move in this direction.
direction A numpy vector of size 7 specifying the desired direction for joints to move speed_limit A scalar float specifying the maximum feasible joint speed step_time A scalar float specifying the step time
Definition at line 986 of file pr2_arm_kinematics.py.
| def goto_target | ( | self, | |
phi = None, |
|||
optimize = False, |
|||
show = False |
|||
| ) |
Finds the inverse kinematic solution for the given redundant parameter phi is phi is not feasible, the solution corresponding to the closest phi is returned. If argument optimize is True, the solution will be optimized so that the joint values will be as close as possible to self.config.qm If phi is not given, current q[0] will be selected as phi The new joint angles will be set if all the kinematic equations are satisfied. All kinematic parameters will be updated.
Definition at line 1026 of file pr2_arm_kinematics.py.
| def grown_phi | ( | self, | |
| eta, | |||
k = 0.99, |
|||
respect_js = False |
|||
| ) |
protected
grows the redundant parameter by eta (adds eta to the current phi=q[0] and returns the new value for phi 1 - this function does NOT set the configuration so the joint values do not change 2 - if the grown phi is not in the feasible interval Delta, it will return the closest point in the range with a safety coefficient k so that new phi = old phi + eta : if eta in Delta new phi = old phi + k*Delta_h : if eta > Delta_h new phi = old phi + k*Delta_l : if eta < Delta_l
Definition at line 753 of file pr2_arm_kinematics.py.
| def IK_config | ( | self, | |
| phi | |||
| ) |
Finds the solution of the Inverse Kinematic problem for given redundant parameter "phi"
In case of redundant solutions, the one corresponding to the lowest objective function is selected.
property ofuncode specifies the objective function:
ofuncode = 0 (Default) the solution closest to current joint angles will be selected
ofuncode = 1 the solution corresponding to the lowest midrange distance is selected
This function does NOT set the configuration so the joints do not change
Definition at line 1244 of file pr2_arm_kinematics.py.
| def ik_direction | ( | self, | |
phi = None, |
|||
optimize = False |
|||
| ) |
Solves the position based inverse kinematics and returns a direction in the jointspace towards the solution (joint correction)
| phi | A scalar float specifying the desired value of the redundant parameter. If set as None (default), the current value of the shoulder-pan joint is replaced. |
| optimize | A boolean If True the IK solution minimize the defined cost function, otherwise the closest solution will be returned. The default is False. |
Definition at line 970 of file pr2_arm_kinematics.py.
| def in_target | ( | self, | |
norm_precision = 0.01 |
|||
| ) |
Use this function to check if the endeffector is in the target pose or not.
| norm_precision | A float specifying the required precision for the norm of both position and orientation error |
Definition at line 633 of file pr2_arm_kinematics.py.
| def joint_redundancy_jacobian | ( | self | ) |
Use this function to get the Joint Redundancy Jacobian vector of the arm.
Since the robot arm has 7 DOF, there is only one redundant parameter. In this case, the Joint Redundancy Jacobian (JRJ) is a vector of 7 elements:
\[ J_i = \frac {\partial \theta_i }{\partial \phi} \]
This function should be called when the endeffector is in target. Otherwise, the output will not be correct.
| None |
Definition at line 729 of file pr2_arm_kinematics.py.
| def js_project | ( | self, | |
| pos_traj, | |||
ori_traj = None, |
|||
phi_start = 0.0, |
|||
phi_end = None, |
|||
relative = True, |
|||
traj_capacity = 2, |
|||
traj_type = 'regular' |
|||
| ) |
projects the given taskspace pose trajectory into the jointspace using analytic inverse kinematics. The phase starts from phi_start and added by self.dt in each step. at any time, if a solution is not found, the process stops
Definition at line 1307 of file pr2_arm_kinematics.py.
| def move_joints_towards | ( | self, | |
| direction, | |||
| max_speed, | |||
step_time = 0.1 |
|||
| ) |
Definition at line 995 of file pr2_arm_kinematics.py.
| def moveto_optimal | ( | self, | |
max_speed = genmath.infinity, |
|||
step_time = 0.1 |
|||
| ) |
Moves the joints within the solution manifold towards optimum configuration.
The optimization is based on the defined cost function. The joint change respects given joint speed limit max_speed A scalar float value specifying the maximum joint speed in Radians per second. The default is infinity A scalar float value specifying time to reach or step time. The default value is 0.1 sec.
Definition at line 903 of file pr2_arm_kinematics.py.
| def moveto_target | ( | self, | |
optimize = False |
|||
| ) |
Definition at line 999 of file pr2_arm_kinematics.py.
| def optimal_config | ( | self, | |
show = False |
|||
| ) |
Finds the optimal value for the redundant parameter \( \phi \) that minimizes the cost function and returns the joints corresponding with optimal redundant parameter value.
This function does not change the object configuration.
| show | If True, the values of redundant parameter and the objective function are printed on the console |
Definition at line 851 of file pr2_arm_kinematics.py.
| def permission_set_position | ( | self | ) |
This function finds and returns the set from which q0 is allowed to be chosen so that joint angles q0 , q2 and q3 in the analytic solution to the inverse kinematic problem are in their range. Consider that being q0 in the permission set, does not guarantee that all q0, q2 and q3 are in their ranges, but it means that if q0 is out of permission set, one of the joints q0, q2 or q3 will definitely be out of their feasible range. In other words, q0 being in perm. set, is a necessary but not sufficient condition for three joints q0, q2 and q3 to be in their range. Permission set is broader than "confidence set" which ensures all q0, q2 and q3 to be in range. (has both necessary and sufficient conditions) The output depends on the defined joint limits and the desired endeffector pose but does not depend on the current position of the joints.
Definition at line 1388 of file pr2_arm_kinematics.py.
| def pose_metric | ( | self | ) |
Use this function to get the displacement between the actual and desired wrist poses The metric returns sum of the norms of position difference Nd Frobenus norm of the orientation difference.
\[ d(x, x_d) = \Sigma_{i = 1}^{3}(x_i - x_{di})^2 + \Sigma_{i = 1}^{3} \Sigma_{j = 1}^{3}(r_{ij} - r_{dij})^2 \]
| None |
Definition at line 842 of file pr2_arm_kinematics.py.
| def position_permission_workspace | ( | self, | |
| fixed | |||
| ) |
Use this method to get the permission range of x, y and z of the endeffector.
If the desired position for the endeffector(wrist joint center) is outside the permission range, then there is definitely no solution for the IK problem. The inverse of this conditional statement is not necessarily true.
| fixed | An array of booleans of size 4, specifying if the corresponding joint is fixed or free The array size is 4 because only the first four joints ( \( q_0, q_1, q_2, q_3 \)) influence the position of the EE |
Example:
Definition at line 787 of file pr2_arm_kinematics.py.
| def restore_config | ( | self, | |
| q_old | |||
| ) |
returns the current configuration and sets the given configuration.
This function is used to restore the saved old configuration
| q_old | A numpy vector of size 7 containing the arm joint values to be set |
Definition at line 830 of file pr2_arm_kinematics.py.
| def set_config | ( | self, | |
| qd | |||
| ) |
Sets the robot configuration to the desired given joint values.
| qd | A numpy array of 7 elements containing the values of the given joints |
Example:
arm = PR2_ARM()
qd = np.zeros(7)
arm.set_config(qd)
print arm.config.q
print arm.wrist_position()
print arm.wrist_orientation()
Definition at line 498 of file pr2_arm_kinematics.py.
| def set_target | ( | self, | |
| target_position, | |||
| target_orientation | |||
| ) |
Sets the endeffector target to a desired position and orientation.
| target_position | A numpy array of 3 elements specifying the desired endeffector position |
| target_orientation | A \( 3 \times 3 \) numpy rotation matrix representing the desired endeffector orientation |
Definition at line 531 of file pr2_arm_kinematics.py.
| def sync_from_magiks | ( | self | ) |
Definition at line 1384 of file pr2_arm_kinematics.py.
| def ts_project | ( | self, | |
| js_traj, | |||
phi_start = 0.0, |
|||
phi_end = None |
|||
| ) |
projects the given jointspace trajectory into the taskspace The phase starts from phi_start and added by delta_phi in each step. if at any time the joint values are not feasible, the process is stopped.
Definition at line 1278 of file pr2_arm_kinematics.py.
| def wrist_orientation | ( | self | ) |
Use this function to get the current orientation of the gripper(Endeffector).
| None |
Definition at line 598 of file pr2_arm_kinematics.py.
| def wrist_position | ( | self | ) |
Use this function to get the current position of the wrist joint center.
| None |
Returns the cartesian coordiantes of the origin of the wrist. The origin of the wrist is the wrist joint center
Definition at line 570 of file pr2_arm_kinematics.py.
| a0 |
A float specifying the value of \( a_0 \) in the DH parameters of the arm.
Definition at line 408 of file pr2_arm_kinematics.py.
| additional_dims |
protected
Definition at line 437 of file pr2_arm_kinematics.py.
| config |
An instance of class PR2_ARM_CONFIGURATION() containing the jointspace properties and methods of the arm.
Definition at line 397 of file pr2_arm_kinematics.py.
| d2 |
A float specifying the value of \( d_2 \) in the DH parameters of the arm.
Definition at line 411 of file pr2_arm_kinematics.py.
| d4 |
A float specifying the value of \( d_4 \) in the DH parameters of the arm.
Definition at line 414 of file pr2_arm_kinematics.py.
| Delta |
Delta A variable of type Interval() containing the feasible interval for the growth of redundant parameter ( \( \Delta \phi \))
Definition at line 475 of file pr2_arm_kinematics.py.
| dt |
Definition at line 416 of file pr2_arm_kinematics.py.
| E |
protected
Definition at line 464 of file pr2_arm_kinematics.py.
| F |
protected
Definition at line 467 of file pr2_arm_kinematics.py.
| ik |
Definition at line 484 of file pr2_arm_kinematics.py.
| in_target_flag |
Definition at line 450 of file pr2_arm_kinematics.py.
| JRJ |
A numpy array containing the Joint Redundancy Jacobian of the arm.
The Joint Redundancy Jacobian represents the derivatives of the joints w.r.t. the redundant parameter
\[ J_{ij} = \frac {\partial \theta_i }{\partial \phi_j} \]
Definition at line 461 of file pr2_arm_kinematics.py.
| l |
Definition at line 424 of file pr2_arm_kinematics.py.
| l_ew |
Definition at line 441 of file pr2_arm_kinematics.py.
| l_se |
protected
Definition at line 440 of file pr2_arm_kinematics.py.
| magiks_running |
Definition at line 485 of file pr2_arm_kinematics.py.
| max_ja |
Definition at line 418 of file pr2_arm_kinematics.py.
| max_js |
Definition at line 417 of file pr2_arm_kinematics.py.
| ofuncode |
An integer between 0 and 1 specifying the objective function code.
Definition at line 444 of file pr2_arm_kinematics.py.
| orientation_respected |
A boolean specifying if orientation of the endeffector should be respected or not in finding the Inverse Kinematic solution.
Definition at line 392 of file pr2_arm_kinematics.py.
| Phi |
Phi A variable of type Interval() containing the feasible interval for the redundant parameter \( \phi \) or the first joint angle (Shoulder Pan Joint)
Definition at line 471 of file pr2_arm_kinematics.py.
| Rd |
A \( 3 \times 3 \) rotation matrix specifying the desired endeffector orientation for the IK problem.
Recommended not to be set directly. Always use function set_target() to change this property.
Definition at line 401 of file pr2_arm_kinematics.py.
| silent |
Definition at line 394 of file pr2_arm_kinematics.py.
| target_parameters |
Definition at line 556 of file pr2_arm_kinematics.py.
| use_magiks |
Definition at line 489 of file pr2_arm_kinematics.py.
| wrist_orientation_matrix |
A \( 3 \times 3 \) numpy matrix representing the orientation of the endeffector (gripper) w.r.t.
the torso Recommended to be read only. Calling function wrist_orientation() changes the value of this property.
Definition at line 454 of file pr2_arm_kinematics.py.
| wrist_position_vector |
An numpy array of size 3 containing the position of the wrist joint center w.r.t.
the shoulder pan joint center in torso coordinate system Recommended to be read only. Calling function wrist_position() changes the value of this property.
Definition at line 448 of file pr2_arm_kinematics.py.
| xd |
A numpy vector of size 3 specifying the desired endeffector position for the IK problem.
Recommended not to be set directly. Always use function set_target() to change this property.
Definition at line 405 of file pr2_arm_kinematics.py.
1.8.8