def __init__(self,
name,
type,
parent,
child,
origin=None,
axis=None,
calibration=None,
dynamics=None,
limit=None,
safety_controller=None,
mimic=None,
**kwargs):
if type not in (Joint.SUPPORTED_TYPES):
raise ValueError('Unsupported joint type: %s' % type)
super(Joint, self).__init__()
self.name = name
self.type = Joint.SUPPORTED_TYPES.index(type)
self.parent = parent if isinstance(parent,
ParentLink) else ParentLink(parent)
self.child = child if isinstance(child,
ChildLink) else ChildLink(child)
self.origin = origin or Origin.from_euler_angles([0., 0., 0.])
self.axis = axis or Axis()
self.calibration = calibration
self.dynamics = dynamics
self.limit = limit
self.safety_controller = safety_controller
self.mimic = mimic
self.attr = kwargs
self.child_link = None
self.position = 0
def data(self, data):
self.geometry = Geometry.from_data(data['geometry'])
self.origin = Origin.from_data(
data['origin']) if data['origin'] else None
self.name = data['name']
self.attr = _attr_from_data(data['attr'])
self.init_transformation = Transformation.from_data(
data['init_transformation']
) if data['init_transformation'] else None
self.current_transformation = Transformation.from_data(
data['current_transformation']
) if data['current_transformation'] else None
def _get_geometry_and_origin(primitive):
shape = None
origin = None
if isinstance(primitive, compas.geometry.Box):
shape = Box.from_geometry(primitive)
origin = Origin(*primitive.frame)
if isinstance(primitive, compas.geometry.Capsule):
shape = Capsule.from_geometry(primitive)
point = primitive.line.midpoint
normal = primitive.line.vector
plane = Plane(point, normal)
origin = Origin.from_plane(plane)
if isinstance(primitive, compas.geometry.Cylinder):
shape = Cylinder.from_geometry(primitive)
origin = Origin.from_plane(primitive.circle.plane)
if isinstance(primitive, compas.geometry.Sphere):
shape = Sphere.from_geometry(primitive)
origin = Origin(primitive.point, [1, 0, 0], [0, 1, 0])
if not shape:
raise Exception('Unrecognized primitive type {}'.format(
primitive.__class__))
geometry = Geometry(shape)
return geometry, origin
def data(self, data):
self.name = data['name']
self.type = Joint.SUPPORTED_TYPES.index(data['type'])
self.parent = ParentLink.from_data(data['parent'])
self.child = ChildLink.from_data(data['child'])
self.origin = Origin.from_data(data['origin']) if data['origin'] else None
self.axis = Axis.from_data(data['axis']) if data['axis'] else None
self.calibration = Calibration.from_data(data['calibration']) if data['calibration'] else None
self.dynamics = Dynamics.from_data(data['dynamics']) if data['dynamics'] else None
self.limit = Limit.from_data(data['limit']) if data['limit'] else None
self.safety_controller = SafetyController.from_data(data['safety_controller']) if data['safety_controller'] else None
self.mimic = Mimic.from_data(data['mimic']) if data['mimic'] else None
self.attr = _attr_from_data(data['attr'])
self.position = data['position']
def __init__(self,
name,
type,
parent,
child,
origin=None,
axis=None,
calibration=None,
dynamics=None,
limit=None,
safety_controller=None,
mimic=None,
**kwargs):
if type not in (Joint.SUPPORTED_TYPES):
raise ValueError('Unsupported joint type: %s' % type)
self.name = name
self.type = Joint.SUPPORTED_TYPES.index(type)
self.parent = parent if isinstance(parent,
ParentLink) else ParentLink(parent)
self.child = child if isinstance(child,
ChildLink) else ChildLink(child)
self.origin = origin or Origin.from_euler_angles([0., 0., 0.])
self.axis = axis or Axis()
self.calibration = calibration
self.dynamics = dynamics
self.limit = limit
self.safety_controller = safety_controller
self.mimic = mimic
self.attr = kwargs
self.child_link = None
self.position = 0
switcher = {
Joint.REVOLUTE: self.calculate_revolute_transformation,
Joint.CONTINUOUS: self.calculate_continuous_transformation,
Joint.PRISMATIC: self.calculate_prismatic_transformation,
Joint.FIXED: self.calculate_fixed_transformation,
Joint.FLOATING: self.calculate_floating_transformation,
Joint.PLANAR: self.calculate_planar_transformation
}
# set the transformation function according to the type
self.calculate_transformation = switcher.get(self.type)
def __init__(self,
name,
type,
parent,
child,
origin=None,
axis=None,
calibration=None,
dynamics=None,
limit=None,
safety_controller=None,
mimic=None,
**kwargs):
type_idx = Joint.SUPPORTED_TYPES.index(type) if isinstance(
type, str) else type
if type_idx not in range(len(Joint.SUPPORTED_TYPES)):
raise ValueError('Unsupported joint type: %s' % type)
super(Joint, self).__init__()
self.name = name
self.type = type_idx
self.parent = parent if isinstance(parent,
ParentLink) else ParentLink(parent)
self.child = child if isinstance(child,
ChildLink) else ChildLink(child)
self.origin = origin or Origin.from_euler_angles([0., 0., 0.])
self.axis = axis or Axis()
self.calibration = calibration
self.dynamics = dynamics
self.limit = limit
self.safety_controller = safety_controller
self.mimic = mimic
self.attr = kwargs
self.child_link = None
self.position = 0
# The following are world-relative frames representing the origin and the axis, which change with
# the joint state, while `origin` and `axis` above are parent-relative and static.
self.current_origin = self.origin.copy()
self.current_axis = self.axis.copy()
def add_joint(self,
name,
type,
parent_link,
child_link,
origin=None,
axis=None,
limit=None,
**kwargs):
"""Adds a joint to the robot model.
Provides an easy way to programmatically add a joint to the robot model.
Parameters
----------
name : str
The name of the joint
type : int
The joint type, e.g. Joint.REVOLUTE
parent_link : :class:`Link`
The joint's parent link.
child_link : :class:`Link`
The joint's child link.
origin : :class:`compas.geometry.Frame`
The joint's origin frame.
axis : :class:`compas.geometry.Vector`
The joint's axis.
limit : list of 2 float
The lower and upper limits of the joint (used for joint types Joint.REVOLUTE or Joint.PRISMATIC)
Returns
-------
:class:`Joint`
The created `Joint`
Raises
------
ValueError
If the joint name is already used in the chain.
Examples
--------
>>> robot = RobotModel('robot')
>>> parent_link = robot.add_link('link0')
>>> child_link = robot.add_link('link1')
>>> origin = Frame.worldXY()
>>> axis = (1, 0, 0)
>>> j = robot.add_joint("joint1", Joint.CONTINUOUS, parent_link, child_link, origin, axis)
"""
all_joint_names = [j.name for j in self.joints]
if name in all_joint_names:
raise ValueError("Joint name '%s' already used in chain." % name)
if origin:
origin = Origin(origin.point, origin.xaxis, origin.yaxis)
if axis:
axis = Axis('{} {} {}'.format(*list(axis)))
if limit:
lower, upper = limit
limit = Limit(lower=lower, upper=upper)
type_str = Joint.SUPPORTED_TYPES[type]
joint = Joint(name,
type_str,
parent_link.name,
child_link.name,
origin=origin,
axis=axis,
limit=limit,
**kwargs)
self.joints.append(joint)
# Using only part of self._rebuild_tree()
parent_link.joints.append(joint)
child_link.parent_joint = joint
self._links[parent_link.name] = parent_link
self._adjacency[parent_link.name] = [
joint.name for joint in parent_link.joints
]
self._links[child_link.name] = child_link
if not parent_link.parent_joint:
self.root = parent_link
joint.child_link = child_link
self._joints[joint.name] = joint
self._adjacency[joint.name] = [child_link.name]
# Using only part of self._create(link, parent_transformation)
parent_transformation = Transformation()
for item in itertools.chain(parent_link.visual, parent_link.collision):
if not item.init_transformation:
item.init_transformation = parent_transformation
else:
parent_transformation = item.init_transformation
joint._create(parent_transformation)
for item in itertools.chain(child_link.visual, child_link.collision):
item.init_transformation = joint.current_transformation
return joint
def data(self, data):
self.origin = Origin.from_data(
data['origin']) if data['origin'] else None
self.mass = Mass.from_data(data['mass']) if data['mass'] else None
self.inertia = Inertia.from_data(
data['inertia']) if data['inertia'] else None