maddux.robots package¶
Submodules¶
maddux.robots.arm module¶
A robot arm defined by a sequence of DH links
-
class
maddux.robots.arm.
Arm
(links, q0, name, active_links=None, base=None, tool=None)[source]¶ -
end_effector_position
(q=None)[source]¶ Return end effector position
Parameters: q (numpy.ndarray or None) – Config to compute the end effector position for a given 1xN q vector Returns: Position (x, y, z) of end effector Return type: numpy.ndarray
-
end_effector_velocity
()[source]¶ Calculate the end effector velocity of the arm given its current angular velocities.
Returns: Returns linear and angular velocity in each dimension (vx, vy, vz, wx, wy, wz). Return type: np.ndarray
-
fkine
(q=None, links=None)[source]¶ Computes the forward kinematics of the arm using the current joint configuration or a given joint configuration
Parameters: - q (numpy.ndarray or None) – (Optional) 1xN vector of joint configuration to compute the FK on
- links (int or None) – (Optional) Specify which links to run fkine on. for example: links=[1,2,3].
Returns: Homogenous coordinates of point at the end of either the specified list of links, or the end effector
Return type: 4x4 numpy.array
-
get_current_joint_config
()[source]¶ Gets the current joint configuration from the links
Returns: 1xN vector of current joint config Return type: numpy.ndarray
-
hold
(obj)[source]¶ Hold a specific object
Parameters: obj (maddux.objects.DynamicObject) – Object to be held Return type: None
-
ikine
(p, num_iterations=1000, alpha=0.1)[source]¶ Computes the inverse kinematics to find the correct joint configuration to reach a given point
Parameters: Returns: 1xN vector of the joint configuration for given point p.
Return type: numpy.ndarray
-
is_in_collision
(env_object)[source]¶ Checks if the arm is in collision with a given object
Parameters: env_object (maddux.Objects.StaticObject) – The object to check for collisions with Returns: Whether you hit the env_object Return type: bool
-
jacob0
(q=None)[source]¶ Calculates the jacobian in the world frame by finding it in the tool frame and then converting to the world frame.
Parameters: q (numpy.ndarray) – (Optional) 1xN joint configuration to compute the jacobian on Returns: 6xN Jacobian in the world frame Return type: numpy.matrix
-
jacobn
(q=None)[source]¶ Calculates the jacobian in the tool frame
Parameters: q (1xN numpy.ndarray) – (Optional) 1xN joint configuration to compute the jacobian on Returns: 6xN Jacobian in the tool frame Return type: numpy.matrix
-
plot
(ax)[source]¶ Plot our robot into given axes
Parameters: ax (matplotlib.axes) – axes of plot Return type: None
-
release
(object_idx=None)[source]¶ Release one or all currently held objects
Parameters: object_idx – (Optional) index of object to release :type object_idx = int or None
Return type: None
-
save_path
(filename)[source]¶ Save the current path to a file :param filename: Filename to save joint config path :type filename: str
-
update_angles
(new_angles, save=False)[source]¶ Updates all the link’s angles
Parameters: - new_angles (numpy.ndarray) – 1xN vector of new link angles
- save – Flag that determines if the update is cached
- save – bool
Return type:
-
update_link_angle
(link, new_angle, save=False)[source]¶ Updates the given link’s angle with the given angle
Parameters: Return type:
-
maddux.robots.link module¶
A Link object holds all information related to a robot link such as the DH parameters and position in relation to the world.
-
class
maddux.robots.link.
Link
(theta, offset, length, twist, q_lim=None, max_velocity=30.0, link_size=0.1, connector_size=0.1)[source]¶ -
compute_transformation_matrix
(q)[source]¶ Transformation matrix from the current theta to the new theta
Parameters: q (int) – the new theta Returns: Transformation matrix from current q to provided q Return type: 4x4 numpy matrix
-
is_in_collision
(env_object)[source]¶ Checks if the arm is in collision with a given static object
Parameters: env_object (maddux.objects.StaticObject) – The object to check for collisions with Returns: Whether link hits the provided env_object Return type: bool
-
plot
(ax)[source]¶ Plots the link on the given matplotlib figure
Parameters: ax (matplotlib.axes) – Figure to plot link upon Return type: None
-
maddux.robots.predefined_robots module¶
-
maddux.robots.predefined_robots.
noodle_arm
(seg_lens, q0, base=None)[source]¶ Creates a complex arm with 10 segments
Parameters: - seg_lens (numpy.ndarray) – 1x10 vector of lengths of each sement
- q0 (numpy.ndarray) – 1xN vector of the starting joint configuration
- base (numpy.ndarray or None) – (Optional) optional (x, y, z) base location of arm
Returns: “Noodle” arm
Return type: maddux.robot.Arm
maddux.robots.utils module¶
Random collection of utilities for the robots to use
-
maddux.robots.utils.
create_homogeneous_transform_from_point
(p)[source]¶ Create a homogeneous transform to move to a given point
Parameters: p (numpy.ndarray) – The (x, y, z) point we want our homogeneous tranform to move to Returns: 4x4 Homogeneous Transform of a point Return type: numpy.matrix
-
maddux.robots.utils.
create_point_from_homogeneous_transform
(T)[source]¶ Create a point from a homogeneous transform
Parameters: T (numpy matrix) – The 4x4 homogeneous transform Returns: The (x, y, z) coordinates of a point from a transform Return type: np.ndarray
-
maddux.robots.utils.
get_rotation_from_homogeneous_transform
(transform)[source]¶ Extract the rotation section of the homogeneous transformation
Parameters: transform (numpy.ndarray) – The 4x4 homogeneous transform to extract the rotation matrix from. Returns: 3x3 Rotation Matrix Return type: numpy.matrix