limit=(-0.5 * math.pi, +0.5 * math.pi))
hand_joint = robot.add_joint('wrist-hand',
Joint.CONTINUOUS,
wrist,
hand,
origin=Frame(Point(0, 0, 0), Vector(1, 0, 0),
Vector(0, 1, 0)),
axis=Vector(0, 0, 1))
# ==============================================================================
# States
# ==============================================================================
names = robot.get_configurable_joint_names()
joints = list(robot.iter_joints())
motions = []
space = list(linspace(0, 1, 100))
for joint in joints:
if joint.limit:
lower = joint.limit.lower
upper = joint.limit.upper
else:
lower = 0
upper = 2 * math.pi
motion = remap_values(space, target_min=lower, target_max=upper)
motions.append(motion)
for values in zip(*motions):
hand_joint = robot.add_joint('wrist-hand',
Joint.CONTINUOUS,
wrist,
hand,
origin=Frame(Point(0, 0, 0), Vector(1, 0, 0),
Vector(0, 1, 0)),
axis=Vector(0, 0, 1))
# ==============================================================================
# State
# ==============================================================================
frames = []
axes = []
for joint in robot.iter_joints():
frame = joint.origin
frame.name = joint.name
frames.append(frame)
axis = joint.axis
axis.name = joint.name
axes.append(axis)
# ==============================================================================
# Visualization
# ==============================================================================
for frame in frames:
artist = FrameArtist(frame,
scale=0.3,
