MAGIKS
1.1
Manipulator General Inverse Kinematic Solver
|
This class introduces a data structure for complete kinematics of a PR2 robot which can sychronize itself with the real robot or the robot in simulation. More...
Public Member Functions | |
def | __init__ |
Class Constructor. More... | |
def | trunk_synced |
def | body_synced |
def | rarm_synced |
Use this function to check if the right arm of the object is synced with the right arm of the real robot. More... | |
def | larm_synced |
Use this function to check if the left arm of the object is synced with the left arm of the real robot. More... | |
def | say |
Use this function if you want the robot to say something. More... | |
def | synced |
Use this function to check if the robot is synchronized with the object. More... | |
def | sync_object |
Synchronizes the object with the robot and verifies the equity of forward kinematics for both the right and left arm endeffectors. More... | |
def | sync_body |
Synchronizes the position and orientation of the real robot body or robot in simulation with the object. More... | |
def | set_config_synced |
def | sync_rarm |
Synchronizes the right arm of the real robot or robot in simulation with the right arm of the object. More... | |
def | sync_larm |
Synchronizes the left arm of the real robot or robot in simulation with the left arm of the object. More... | |
def | sync_robot |
Synchronizes the real robot or robot in simulation with the object. More... | |
def | rarm_target |
Solves the Inverse Kinematics for the right arm with given redundant parameter \( \phi \) and takes the right arm endeffector to the target specified by properties self.rarm.xd and self.rarm.Rd . More... | |
def | larm_target |
Solves the Inverse Kinematics for the left arm with given redundant parameter \( \phi \) and takes the right arm endeffector to the target specified by properties self.larm.xd and self.larm.Rd . More... | |
def | arm_target |
Solves the Inverse Kinematics for the reference arm with given redundant parameter \( \phi \) The instructions are exactly the same as functions self.rarm_target() and self.larm_target() More... | |
def | reach_target |
def | reference_arm |
def | arm_back |
Moves the reference arm wrist in backward direction maintaining the gripper orientation. More... | |
def | arm_forward |
Moves the reference arm wrist in forward direction maintaining the gripper orientation. More... | |
def | arm_down |
Moves the reference arm wrist in downward direction maintaining the gripper orientation. More... | |
def | arm_up |
Moves the reference arm wrist in upward direction maintaining the gripper orientation. More... | |
def | arm_right |
Moves the reference arm wrist to the right maintaining the gripper orientation. More... | |
def | arm_left |
Moves the reference arm wrist to the left maintaining the gripper orientation. More... | |
def | arm_left_down |
Moves the reference arm wrist to the left and downward direction maintaining the gripper orientation. More... | |
def | arm_left_up |
Moves the reference arm wrist to both left and upward directions maintaining the gripper orientation. More... | |
def | arm_right_up |
def | arm_right_down |
def | arm_arc |
Starting from the current wrist position , the arm wrist draws an arc around the given center with the given angle in the plane specified by the given normal vector. More... | |
def | arm_orient |
Changes the orientation of the reference arm gripper to a desired direction with out changing the wrist position. More... | |
def | front_line_tilt |
def | front_line_base |
def | arm_trajectory |
Runs a given arm task-space pose trajectory on the robot. More... | |
def | activate_kc |
Activates the Kinematic Control Service. More... | |
def | deactivate_kc |
Deactivates the Kinematic Control Service. More... | |
def | kc_service |
def | arm_track |
Public Attributes | |
r_arm_joint_names | |
l_arm_joint_names | |
max_wait_time | |
A dictionary specifying the maximum waiting time when you choose to wait for any motion to finish. More... | |
arm_speed | |
A float specifying the speed of arm gripper in the operational space in m/sec. More... | |
arm_max_speed | |
base_laser_scan_range | |
tilt_laser_scan_range | |
larm_reference | |
control_mode | |
kc_service_cnt | |
prev_qdot | |
kc_gain | |
kc_time | |
t0 | |
prev_err | |
kc_ptd | |
kc_otd | |
kc_pta | |
kc_ota | |
kc_jtd | |
kc_jta | |
This class introduces a data structure for complete kinematics of a PR2 robot which can sychronize itself with the real robot or the robot in simulation.
An instance of this class is an object supported by all kinematic functions inherited from class packages.nima.robotics.kinematics.special_geometries.pr2.pr2_kinematics.PR2() containing additional methods in order to actuate, move and control the real robot and/or get the sensory data from it The object can synchronize itself with the robot in both forward and inverse directions via pyride interface engine. In other words, any changes in the status and configuration of the object can be directly applied to the real robot or robot in simulation and vice versa. Forward synchronization is to actuate the robot to the configuration of the object and inverse synchronization is to set the object with the configuration of the real robot.
Definition at line 171 of file pyride_synchronizer.py.
def __init__ | ( | self, | |
run_magiks = False |
|||
) |
Class Constructor.
Definition at line 175 of file pyride_synchronizer.py.
def activate_kc | ( | self | ) |
Activates the Kinematic Control Service.
Definition at line 944 of file pyride_synchronizer.py.
def arm_arc | ( | self, | |
center = np.array([0.0, -0.05 , |
|||
angle = math.pi , |
|||
normal = np.array([1.0, 0.0 , |
|||
N = 100 , |
|||
wait = True |
|||
) |
Starting from the current wrist position , the arm wrist draws an arc around the given center with the given angle in the plane specified by the given normal vector.
center | A numpy vector of 3 elements specifying the position of the arc center w.r.t. the starting point and should be in the coordinate system of the gripper |
angle | A float specifying the arc angle in radians (The default value is \( \pi \)) |
N | An integer specifying the number of key points in the arc trajectory (The default value is 100) |
normal | A numpy vector of 3 elements specifying a vector perpendicular to the arc plane. The given vector should be in the coordinate system of the gripper and must be perpendicular to the vector given by parameter center (The default value is unit vector \( i = [1.0, 0.0, 0.0] \) specifying the forward direction w.r.t. the gripper) |
wait | A boolean:
|
Definition at line 755 of file pyride_synchronizer.py.
def arm_back | ( | self, | |
dx = 0.1 , |
|||
relative = False , |
|||
wait = True |
|||
) |
Moves the reference arm wrist in backward direction maintaining the gripper orientation.
dx | A float specifying the distance (in meters) by which the wrist should move in backward direction |
relative | A boolean for selecting the orientation with respect to which the backward direction is defined If True, the backward direction is defined relative to the gripper orientation if False the absolute backward direction (with respect to the robot trunk) is considered |
Definition at line 535 of file pyride_synchronizer.py.
def arm_down | ( | self, | |
dx = 0.1 , |
|||
relative = False , |
|||
wait = True |
|||
) |
Moves the reference arm wrist in downward direction maintaining the gripper orientation.
dx | A float specifying the distance (in meters) by which the wrist should move in downward direction |
relative | A boolean for selecting the orientation with respect to which the downward direction is defined If True, the downward direction is defined relative to the gripper orientation If False the absolute downward direction (with respect to the robot trunk) is considered |
Definition at line 577 of file pyride_synchronizer.py.
def arm_forward | ( | self, | |
dx = 0.1 , |
|||
relative = False , |
|||
wait = True |
|||
) |
Moves the reference arm wrist in forward direction maintaining the gripper orientation.
dx | A float specifying the distance (in meters) by which the wrist should move in forward direction |
relative | A boolean for selecting the orientation with respect to which the forward direction is defined If True, the forward direction is defined relative to the gripper orientation If False the absolute forward direction (with respect to the robot trunk) is considered |
Definition at line 557 of file pyride_synchronizer.py.
def arm_left | ( | self, | |
dx = 0.1 , |
|||
relative = False , |
|||
wait = True |
|||
) |
Moves the reference arm wrist to the left maintaining the gripper orientation.
The speed of motion is set by property self.arm_speed
dx | A float specifying the distance (in meters) by which the wrist should move to the left |
relative | A boolean for selecting the orientation with respect to which the left direction is defined If True, the left direction is defined relative to the gripper orientation, if False the absolute left direction (with respect to the robot trunk) is considered |
Definition at line 641 of file pyride_synchronizer.py.
def arm_left_down | ( | self, | |
dx = 0.1 , |
|||
dy = 0.1 , |
|||
relative = False , |
|||
wait = True |
|||
) |
Moves the reference arm wrist to the left and downward direction maintaining the gripper orientation.
The speed of motion is set by property self.arm_speed
dx | A float specifying the distance (in meters) by which the wrist should move to the left |
dy | A float specifying the distance (in meters) by which the wrist should move downwards |
relative | A boolean for selecting the orientation with respect to which the left and downward directions are defined If True, the left and downward directions are defined relative to the gripper orientation, if False the absolute left and downward directions (with respect to the robot trunk) are considered |
Definition at line 663 of file pyride_synchronizer.py.
def arm_left_up | ( | self, | |
dx = 0.1 , |
|||
dy = 0.1 , |
|||
relative = False , |
|||
wait = True |
|||
) |
Moves the reference arm wrist to both left and upward directions maintaining the gripper orientation.
The speed of motion is set by property self.arm_speed
dx | A float specifying the distance (in meters) by which the wrist should move to the left |
dy | A float specifying the distance (in meters) by which the wrist should move upwards |
relative | A boolean for selecting the orientation with respect to which the left and upward directions are defined If True, the left and upward directions are defined relative to the gripper orientation, if False the absolute left and upward directions (with respect to the robot trunk) are considered |
Definition at line 687 of file pyride_synchronizer.py.
def arm_orient | ( | self, | |
direction = 'forward' , |
|||
ttr = 2.0 , |
|||
wait = True |
|||
) |
Changes the orientation of the reference arm gripper to a desired direction with out changing the wrist position.
A string specifying the direction. Must be selected among:
'forward', 'backward', 'upward', 'downward', 'right', 'left'
ttr | A float specifying the time to reach the target orientation |
wait | A boolean:
|
Definition at line 837 of file pyride_synchronizer.py.
def arm_right | ( | self, | |
dx = 0.1 , |
|||
relative = False , |
|||
wait = True |
|||
) |
Moves the reference arm wrist to the right maintaining the gripper orientation.
dx | A float specifying the distance (in meters) by which the wrist should move to the right |
relative | A boolean for selecting the orientation with respect to which the direction to the right is defined If True, the right direction is defined relative to the gripper orientation, if False the absolute direction to the right (with respect to the robot trunk) is considered |
moves the arm to the right as much as dx (m) maintaining the orientation
Definition at line 617 of file pyride_synchronizer.py.
def arm_right_down | ( | self, | |
dx = 0.1 , |
|||
dy = 0.1 , |
|||
relative = False , |
|||
wait = True |
|||
) |
moves the arm to the left as much as dx (m) maintaining the orientation
Definition at line 720 of file pyride_synchronizer.py.
def arm_right_up | ( | self, | |
dx = 0.1 , |
|||
dy = 0.1 , |
|||
relative = False , |
|||
wait = True |
|||
) |
moves the arm to the left as much as dx (m) maintaining the orientation
Definition at line 702 of file pyride_synchronizer.py.
def arm_target | ( | self, | |
phi = None , |
|||
ttr = 5.0 , |
|||
wait = True |
|||
) |
Solves the Inverse Kinematics for the reference arm with given redundant parameter \( \phi \) The instructions are exactly the same as functions self.rarm_target() and self.larm_target()
Definition at line 496 of file pyride_synchronizer.py.
def arm_track | ( | self, | |
k = 1.0 , |
|||
delay = 0.1 , |
|||
max_speed = 1.0 , |
|||
relative = True |
|||
) |
Definition at line 1027 of file pyride_synchronizer.py.
def arm_trajectory | ( | self, | |
pos_traj, | |||
ori_traj = None , |
|||
resolution = 50 , |
|||
relative = True , |
|||
wait = True |
|||
) |
Runs a given arm task-space pose trajectory on the robot.
The given trajectory will be tracked by the wrist.
pos_traj | An instance of class packages.nima.robotics.kinematics.task_space.trajectory.Trajectory_Polynomial() specifying the trajectory in the 3D taskspace to be tracked by the arm. |
ori_traj | An instance of class packages.nima.robotics.kinematics.task_space.trajectory.Trajectory_Polynomial() specifying the trajectory in the 3D taskspace to be tracked by the arm. First, projects the given taskspace pose trajectory into the jointspace using both velocity-based and position-based inverse kinematics of the arm and creates a joint trajectory. Then the joint trajectory is given to function run_config_trajectory of pyride_interpreter to run on the robot. |
Definition at line 900 of file pyride_synchronizer.py.
def arm_up | ( | self, | |
dx = 0.1 , |
|||
relative = False , |
|||
wait = True |
|||
) |
Moves the reference arm wrist in upward direction maintaining the gripper orientation.
dx | A float specifying the distance (in meters) by which the wrist should move in upward direction |
relative | A boolean for selecting the orientation with respect to which the upward direction is defined If True, the upward direction is defined relative to the gripper orientation If False the absolute upward direction (with respect to the robot trunk) is considered |
Definition at line 597 of file pyride_synchronizer.py.
def body_synced | ( | self | ) |
Returns True if robot and object body positions are identical
Definition at line 223 of file pyride_synchronizer.py.
def deactivate_kc | ( | self | ) |
Deactivates the Kinematic Control Service.
Definition at line 952 of file pyride_synchronizer.py.
def front_line_base | ( | self | ) |
Returns the front line of tilt scan
Definition at line 870 of file pyride_synchronizer.py.
def front_line_tilt | ( | self | ) |
Returns the front line of tilt scan
Definition at line 859 of file pyride_synchronizer.py.
def kc_service | ( | self, | |
data | |||
) |
Definition at line 957 of file pyride_synchronizer.py.
def larm_synced | ( | self | ) |
Use this function to check if the left arm of the object is synced with the left arm of the real robot.
None |
Definition at line 247 of file pyride_synchronizer.py.
def larm_target | ( | self, | |
phi = None , |
|||
ttr = 5.0 , |
|||
wait = True |
|||
) |
Solves the Inverse Kinematics for the left arm with given redundant parameter \( \phi \) and takes the right arm endeffector to the target specified by properties self.larm.xd and self.larm.Rd .
phi | A float by which you specify the value of the desired redundant parameter (The desired value of shoulder-pan joint). If phi = None, then the optimum phi corresponding to the minimum value of the objective function will be selected. (The default value is None) |
ttr | A float specifying time to reach in seconds. Use this parameter to specify the amount of time needed to reach the target. Obviously this value influences the speed of motion. |
wait | A boolean:
|
Solves the Inverse Kinematics for the left arm with given redundant parameter "phi" and takes the right arm endeffector to the target specified by self.larm.xd and self.larm.Rd if phi = None, then the optimum phi will be selected.
Definition at line 463 of file pyride_synchronizer.py.
def rarm_synced | ( | self | ) |
Use this function to check if the right arm of the object is synced with the right arm of the real robot.
None |
Definition at line 240 of file pyride_synchronizer.py.
def rarm_target | ( | self, | |
phi = None , |
|||
ttr = 5.0 , |
|||
wait = True |
|||
) |
Solves the Inverse Kinematics for the right arm with given redundant parameter \( \phi \) and takes the right arm endeffector to the target specified by properties self.rarm.xd and self.rarm.Rd .
phi | A float by which you specify the value of the desired redundant parameter (The desired value of shoulder-pan joint). If phi = None, then the optimum phi corresponding to the minimum value of the objective function will be selected. (The default value is None) |
ttr | A float specifying time to reach in seconds. Use this parameter to specify the amount of time needed to reach the target. Obviously this value influences the speed of motion. |
wait | A boolean:
|
Definition at line 422 of file pyride_synchronizer.py.
def reach_target | ( | self | ) |
# should change to goto_target() calling the goto_target() function of the super class Solves the IK in the current control mode and takes the robot endeffector to the desired pose specified by self.xd and self.Rd
Definition at line 502 of file pyride_synchronizer.py.
def reference_arm | ( | self | ) |
Definition at line 522 of file pyride_synchronizer.py.
def say | ( | self, | |
s | |||
) |
Use this function if you want the robot to say something.
s | A string what you want the robot to say |
Definition at line 253 of file pyride_synchronizer.py.
def set_config_synced | ( | self, | |
qd, | |||
ttr = 5.0 , |
|||
max_time = 120.0 , |
|||
limb_list = ['body' , |
|||
rarm, | |||
larm | |||
) |
sets the given configuration to the object and synchronizes the robot with the object. If the given joints are feasible, the robot takes the given configuration. The function will return False if the robot can not reach the desired configuration within max_time
Definition at line 314 of file pyride_synchronizer.py.
def sync_body | ( | self, | |
ttr = 5.0 , |
|||
wait = True , |
|||
max_wait_time = 60.0 |
|||
) |
Synchronizes the position and orientation of the real robot body or robot in simulation with the object.
If the body pose is not already synced, it takes the configuration specified by self.q[8:11]
ttr | A float specifying time to reach in seconds |
wait | A boolean:
|
Definition at line 301 of file pyride_synchronizer.py.
def sync_larm | ( | self, | |
ttr = 5.0 , |
|||
wait = True |
|||
) |
Synchronizes the left arm of the real robot or robot in simulation with the left arm of the object.
If the left arm is not already synced, it takes the configuration specified by self.q[11:18] or self.larm.config.q[0:7]
ttr | A float specifying time to reach in seconds |
wait | A boolean:
|
Definition at line 369 of file pyride_synchronizer.py.
def sync_object | ( | self | ) |
Synchronizes the object with the robot and verifies the equity of forward kinematics for both the right and left arm endeffectors.
None |
Definition at line 272 of file pyride_synchronizer.py.
def sync_rarm | ( | self, | |
ttr = 5.0 , |
|||
wait = True |
|||
) |
Synchronizes the right arm of the real robot or robot in simulation with the right arm of the object.
If the right arm is not already synced, it takes the configuration specified by self.q[0:7] or self.rarm.config.q[0:7]
ttr | A float specifying time to reach in seconds |
wait | A boolean:
|
Definition at line 353 of file pyride_synchronizer.py.
def sync_robot | ( | self, | |
ttr = 5.0 , |
|||
wait = True |
|||
) |
Synchronizes the real robot or robot in simulation with the object.
The body, trunk, right and left arms synchronize simultaneously. If each part is already synced, it will not move. If the system is in Free-Base mode, the robot navigates to the position specified by \( x = q[8] \) and \( y = q[9] \) and twists to rotation angle specified by \( \tau = q[10] \). (If the system is in fixed-base mode, the body will not move)
The right arm takes the configuration specified by self.q[0:7] or self.rarm.config.q[0:7]
The left arm takes the configuration specified by self.q[11:18] or self.larm.config.q[0:7]
ttr | A float specifying time to reach in seconds |
wait | A boolean:
|
Definition at line 390 of file pyride_synchronizer.py.
def synced | ( | self, | |
limb_list = ['body' , |
|||
rarm, | |||
larm, | |||
trunk | |||
) |
Use this function to check if the robot is synchronized with the object.
limb_list | An array of strings specifying which parts should be checked. |
Definition at line 259 of file pyride_synchronizer.py.
def trunk_synced | ( | self | ) |
Returns True if robot and object body heights are identical
Definition at line 216 of file pyride_synchronizer.py.
arm_max_speed |
Definition at line 210 of file pyride_synchronizer.py.
arm_speed |
A float specifying the speed of arm gripper in the operational space in m/sec.
Definition at line 208 of file pyride_synchronizer.py.
base_laser_scan_range |
Definition at line 211 of file pyride_synchronizer.py.
control_mode |
Definition at line 394 of file pyride_synchronizer.py.
kc_gain |
Definition at line 947 of file pyride_synchronizer.py.
kc_jta |
Definition at line 989 of file pyride_synchronizer.py.
kc_jtd |
Definition at line 988 of file pyride_synchronizer.py.
kc_ota |
Definition at line 987 of file pyride_synchronizer.py.
kc_otd |
Definition at line 985 of file pyride_synchronizer.py.
kc_pta |
Definition at line 986 of file pyride_synchronizer.py.
kc_ptd |
Definition at line 984 of file pyride_synchronizer.py.
kc_service_cnt |
Definition at line 945 of file pyride_synchronizer.py.
kc_time |
Definition at line 981 of file pyride_synchronizer.py.
l_arm_joint_names |
Definition at line 179 of file pyride_synchronizer.py.
larm_reference |
Definition at line 214 of file pyride_synchronizer.py.
max_wait_time |
A dictionary specifying the maximum waiting time when you choose to wait for any motion to finish.
This time can be different for the motion of different parts of the robot.
max_wait_time['rarm']: Refers to the motion of the right arm (Change in joints q[0] - q[6])
max_wait_time['larm']: Refers to the motion of the left arm (Change in joints q[11] - q[17])
max_wait_time['body']: Refers to the navigation of the body (Change in joints q[8], q[9] , q[10])
max_wait_time['trunk']: Refers to the trunk lifting motion (Change in joint q[7])
Definition at line 191 of file pyride_synchronizer.py.
prev_err |
Definition at line 983 of file pyride_synchronizer.py.
prev_qdot |
Definition at line 946 of file pyride_synchronizer.py.
r_arm_joint_names |
Definition at line 178 of file pyride_synchronizer.py.
t0 |
Definition at line 982 of file pyride_synchronizer.py.
tilt_laser_scan_range |
Definition at line 212 of file pyride_synchronizer.py.