class AlignedKinematicsTest(unittest.TestCase):
"""
Tests aligned kinematics
"""
def setUp(self):
print("<Setup START>")
links = [26, 22, 12.5]
self.kinematics = Kinematics(links, 0, 0, 0, 0)
def test_inverse_aligned_kuka_initial(self):
"""
Tests the following aligned inverse kinematics problem:
1) "rear low" Axis 3 fixed:
0 on base, 1.73 on joint 1, 2.50 on joint 2, -0.75 on joint 3, 0 on joint 4:
35.01157, 0, 25.96866
"""
print("<Test aligned kuka initial START>")
x, y, z = 35.01157, 0, 25.96866
alignment = 0
results = self.kinematics.inverse_aligned(x, y, z, alignment)
print(results)
expected = [0, 1.55, -1.55, 0]
result = all((abs(x - y) < 0.01 for x, y in zip(results, expected)))
self.assertTrue(result)
def test_inverse_aligned_rear_high(self):
"""
Tests the following aligned inverse kinematics problem:
1) "rear low" Axis 3 fixed:
0 on base, 1.6735 on joint 1, -0.8171 on joint 2, -1.0564 on joint 3, 0 on joint 4:
24, 0, 40
"""
print("<Test aligned kuka initial START>")
x, y, z = 24, 0, 40
alignment = -0.2
results = self.kinematics.inverse_aligned(x, y, z, alignment)
print(results)
expected = [0, 1.6735, -0.8171, -1.0564]
result = all((abs(x - y) < 0.01 for x, y in zip(results, expected)))
self.assertTrue(result)
def test_inverse_align_too_far(self):
"""
Tests the following inverse kinematics problem:
11) "Way too far":
not reachable
500, 500, 500
"""
print("<Test Inverse too far START>")
x, y, z = 500, 500, 500
alignment = 0
self.assertRaises(CalculationError, self.kinematics.inverse_aligned, x, y, z, alignment)
class Mechanism():
def __init__(self, feature):
self.kinematics = Kinematics(table)
self.qstr = [
'q{0}'.format(i + 1) for i in range(len(self.kinematics.ajoints))
]
add_rev = lambda s: feature.addProperty("App::PropertyAngle", s,
"Joint Values", s)
add_prism = lambda s: feature.addProperty("App::PropertyDistance", s,
"Joint Values", s)
for jnt, qstr in zip(self.kinematics.ajoints, self.qstr):
if (jnt.isrevolute()):
add_rev(qstr)
elif (jnt.isprismatic()):
add_prism(qstr)
feature.Proxy = self
def onChanged(self, feature, prop):
if prop.startswith('q'):
self.set_joint_values(feature)
self.kinematics.solve_loops()
self.createShape(feature)
def execute(self, feature):
self.set_joint_values(feature)
self.kinematics.solve_loops()
self.createShape(feature)
def set_joint_values(self, feature):
qlist = [feature.getPropertyByName(s) for s in self.qstr]
for jnt, q in zip(self.kinematics.ajoints, qlist):
if (jnt.isrevolute()):
from math import pi
q *= pi / 180
jnt.q = q
def createShape(self, feature):
# TODO: use real shapes
comp = Part.Compound([])
for jnt in self.kinematics.joints:
m, Pjminus1 = self.kinematics.get_joint_transform(jnt)
Pj = Base.Vector(m.A14, m.A24, m.A34)
v = Pj - Pjminus1
if (v.Length > 0):
body_shape = Part.makeCylinder(d_body, v.Length, Pjminus1, v)
comp.add(body_shape)
if (jnt.isrevolute()):
joint_shape = Part.makeCylinder(d_rev, l_rev,
Base.Vector(0, 0, -l_rev / 2))
elif (jnt.isprismatic()):
joint_shape = Part.makeBox(
d_prism, d_prism, l_prism,
Base.Vector(-d_prism / 2, -d_prism / 2, -l_prism / 2))
if not (jnt.isfixed()):
joint_shape.Matrix = m
comp.add(joint_shape)
# TODO: add the frame for each joint and add a property to switch
# frame display on/off.
feature.Shape = comp
class Mechanism():
def __init__(self, feature):
self.kinematics = Kinematics(table)
self.qstr = ['q{0}'.format(i + 1) for i in range(
len(self.kinematics.ajoints))]
add_rev = lambda s: feature.addProperty("App::PropertyAngle",
s, "Joint Values", s)
add_prism = lambda s: feature.addProperty("App::PropertyDistance",
s, "Joint Values", s)
for jnt, qstr in zip(self.kinematics.ajoints, self.qstr):
if (jnt.isrevolute()):
add_rev(qstr)
elif (jnt.isprismatic()):
add_prism(qstr)
feature.Proxy = self
def onChanged(self, feature, prop):
if prop.startswith('q'):
self.set_joint_values(feature)
self.kinematics.solve_loops()
self.createShape(feature)
def execute(self, feature):
self.set_joint_values(feature)
self.kinematics.solve_loops()
self.createShape(feature)
def set_joint_values(self, feature):
qlist = [feature.getPropertyByName(s) for s in self.qstr]
for jnt, q in zip(self.kinematics.ajoints, qlist):
if (jnt.isrevolute()):
from math import pi
q *= pi / 180
jnt.q = q
def createShape(self, feature):
# TODO: use real shapes
comp = Part.Compound([])
for jnt in self.kinematics.joints:
m, Pjminus1 = self.kinematics.get_joint_transform(jnt)
Pj = Base.Vector(m.A14, m.A24, m.A34)
v = Pj - Pjminus1
if (v.Length > 0):
body_shape = Part.makeCylinder(d_body, v.Length, Pjminus1, v)
comp.add(body_shape)
if (jnt.isrevolute()):
joint_shape = Part.makeCylinder(d_rev, l_rev,
Base.Vector(0, 0, -l_rev / 2))
elif (jnt.isprismatic()):
joint_shape = Part.makeBox(d_prism, d_prism, l_prism,
Base.Vector(-d_prism / 2, -d_prism / 2, -l_prism / 2))
if not(jnt.isfixed()):
joint_shape.Matrix = m
comp.add(joint_shape)
# TODO: add the frame for each joint and add a property to switch
# frame display on/off.
feature.Shape = comp
def __init__(self):
#self.cv_image1 = cv_image1
#self.cv_image2 = cv_image2
self.meter_to_pixel_factor = -1
self.initial_angles = [0, 0, 0, 0]
self.time_previous_step = np.array([rospy.get_time()], dtype='float64')
self.error = np.array([0.0, 0.0, 0.0], dtype='float64')
self.error_d = np.array([0.0, 0.0, 0.0], dtype='float64')
self.kinematics = Kinematics()
print("Init JointState")
def minJerkSetup_now(initial_angles, tf, waypoints, t_array=None):
num_waypoints = waypoints.shape[1]
try:
if t_array == None:
del_t = float(tf) / float(num_waypoints)
t_array = del_t * np.ones((num_waypoints, 1))
elif not t_array.size == num_waypoints:
raise ValueError('Time array length is incorrect')
elif not tf == np.sum(t_array):
raise ValueError('Time array must add up to final time')
except:
if not t_array.size == num_waypoints:
raise ValueError('Time array length is incorrect')
elif not tf == np.sum(t_array):
raise ValueError('Time array must add up to final time')
joint_constants = namedtuple('joint_constants', 'J1 J2 J3 J4 J5')
joint_const = joint_constants(np.zeros((6, num_waypoints)),
np.zeros((6, num_waypoints)),
np.zeros((6, num_waypoints)),
np.zeros((6, num_waypoints)),
np.zeros((6, num_waypoints)))
x0 = np.zeros((5, 6))
if initial_angles.ndim == 2:
if initial_angles.shape[0] == 5:
initial_angles = initial_angles.T
x0[:, 0] = initial_angles
x0[:, 3] = KMTCS.inverseKinematics(waypoints[0, 0], waypoints[1, 0],
waypoints[2, 0], waypoints[3, 0]).T
t0 = np.zeros((num_waypoints, 1))
tf = np.zeros((num_waypoints, 1))
tf[0] = t_array[0]
for i in range(num_waypoints):
if i > 0:
x0[:, 0] = x0[:, 3]
x0[:,
3] = KMTCS.inverseKinematics(waypoints[0, i], waypoints[1, i],
waypoints[2, i], waypoints[3, i]).T
t0[i] = tf[i - 1]
tf[i] = t0[i] + t_array[i]
joint_const.J1[:, i] = minJerkSetup(x0[0], t0[i], tf[i])
joint_const.J2[:, i] = minJerkSetup(x0[1], t0[i], tf[i])
joint_const.J3[:, i] = minJerkSetup(x0[2], t0[i], tf[i])
joint_const.J4[:, i] = minJerkSetup(x0[3], t0[i], tf[i])
joint_const.J5[:, i] = minJerkSetup(x0[4], t0[i], tf[i])
return joint_const
def __init__(self, feature):
self.kinematics = Kinematics(table)
self.qstr = [
'q{0}'.format(i + 1) for i in range(len(self.kinematics.ajoints))
]
add_rev = lambda s: feature.addProperty("App::PropertyAngle", s,
"Joint Values", s)
add_prism = lambda s: feature.addProperty("App::PropertyDistance", s,
"Joint Values", s)
for jnt, qstr in zip(self.kinematics.ajoints, self.qstr):
if (jnt.isrevolute()):
add_rev(qstr)
elif (jnt.isprismatic()):
add_prism(qstr)
feature.Proxy = self
def __init__(self):
"""
position: init position dict with keys:
- "angle": a X-axis relative angle.
- "point": the first position.
"""
# self._us_sensors = RangeSensor(4)
self._motors = Motors(5)
self._kinematic = Kinematics(6)
self._us = RangeSensor(4)
logging.info('Building the graph map.')
# self._graph = build_graph(robot_diagonal)
logging.info('Finished to build the graph map.')
self._blocking_servo = -1
def test_RC_6DOF(self):
"""Tests forward and inverse kinematics with 6 degrees of freedom in
robot coordinates.
"""
k = Kinematics.from_origin() # test robot coordinates only
k.set_joint2_offset(30)
k.set_joint2_height(30)
k.set_joint4_offset(10)
k.set_arm23_length(150)
k.set_arm35_length(150)
k.set_wrist_length(10)
k.set_endeffector(Transformation.from_identity())
for i in range(1000):
rot_mat = RotationMatrix(np.random.rand(3))
orientation_mat = rot_mat.matrix_at_angle(random.random())
target_location = np.ones(3) * 100 + np.random.rand(3) * 40
expected = Transformation(orientation_mat, target_location, \
calc_inverse=False)
angles = k.inverse(expected)
actual = k.forward(angles)
# numerical errors might occur
#if not np.allclose(target.get_rotation(), orientation_mat):
# print(target.get_rotation() - orientation_mat)
assert np.allclose(actual.get_rotation(), orientation_mat)
assert np.allclose(actual.get_translation(), target_location)
def test_RC_lockJ4(self):
"""Tests forward and inverse kinematics in RC coordinates with joint 4
being locked to zero degrees. Test without end-effector.
"""
k = Kinematics.from_origin() # test robot coordinates only
k.set_joint2_offset(30)
k.set_joint2_height(30)
k.set_joint4_offset(10)
k.set_arm23_length(150)
k.set_arm35_length(150)
k.set_wrist_length(10)
k.set_endeffector(Transformation.from_identity())
rot_mat = RotationMatrix.from_axis('z') # implicitly locks joint 4
for i in range(1000):
orientation_mat = rot_mat.matrix_at_angle(random.random())
target_location = np.ones(3) * 100 + np.random.rand(3) * 40
target = Transformation(orientation_mat, target_location, \
calc_inverse=False)
angles = k.inverse(target)
assert math.isclose(angles[3], 0) # joint 4 is locked to zero
target = k.forward(angles)
assert np.allclose(target.get_rotation(), orientation_mat)
assert np.allclose(target.get_translation(), target_location)
def __init__(self, parent, com_port, baud_rate, max_tries=10):
self.connected = False
self.parent = parent
self.kinematics = Kinematics()
self.comport = com_port
self.baud_rate = baud_rate
self.ser = self.connect(com_port, baud_rate, max_tries)
if self.ser is None:
p = multiprocessing.Process(target=self.retryConnect)
p.start()
self.stateObservers: [StateObserver] = []
self.textObservers: [TextObserver] = []
self.legStateObservers: [LegStateObserver] = []
self.requestState()
def __init__(self, graspit, robot, body, collision_object=None):
default_pose = Pose()
default_pose.position.y = 0.2
default_pose.orientation.w = 1
graspit.clearWorld()
graspit.importRobot(robot)
self._robot_ids = graspit.getBodies().ids
graspit.setRobotPose(default_pose)
graspit.importGraspableBody(body)
self._body_id = graspit.getBodies().ids[-1]
self._collision_object = collision_object
self._collision_object_id = -1
if self._collision_object is not None:
graspit.importObstacle(
self._collision_object['id'],
msg_from_pose(self._collision_object['pose']))
self._collision_object_id = graspit.getBodies().ids[-1]
graspit.toggleCollisions(False, self._collision_object_id,
self._body_id)
self._graspit = graspit
self._robot = robot
self._body = body
self._kinematics = Kinematics('{}/models/robots/{}'.format(
os.environ['GRASPIT'], robot))
def updateParametersUsingGaussNewton(self, parameters):
''' given a set of parameters, return ( newParameters, oldCost ) by doing a single
Gauss-Newton iteration
'''
# cache the number of residual functions
numResidualPairs = self.getNumResidualPairs()
# compute the vector of residuals
self.setCalibrationFromParameters(parameters, self.calibration)
kinematics = Kinematics(self.calibration)
#residuals = flatten( [ self.computeResidualPair( i, kin ) for i in range( numResidualPairs ) ] )
residuals = self.computeVectorOfResiduals(kinematics)
# # log residuals
# logging.getLogger( name='main' ).debug( ' residuals = {!r}'.format( residuals ) )
# calculate the Jacobian of the residuals
jacobian = self.computeJacobianOfResiduals(parameters)
# calculate Moore-Penrose pseudoinverse of the jacobian
jacobianInv = numpy.linalg.pinv(jacobian)
# calculate new parameters using Gauss-Newton step:
# newParms = oldParms + jacobianInv @ residuals
#warning WHY DOES THIS ONLY WORK WITH A MINUS NOT A PLUS?
newParameters = parameters - self.updateParametersScalar * jacobianInv @ residuals
# compute old cost function
oldCost = sum([residual**2 for residual in residuals])
# return
return newParameters, oldCost
def initializeMeasurements(self, measurements, calibration):
''' initialize measurements related member values '''
# remember the actual positions
self.actualPositions = [
self.convertMeasurementToBedPosition(position, calibration)
for position in measurements.actualPositions
]
# compute the target positions
targetPositions = [
self.convertMeasurementToBedPosition(position, calibration)
for position in measurements.targetPositions
]
# create kinematics to calculate chain lengths
kinematics = Kinematics(calibration)
# calculate the chain lengths that were generated from the target positions
# These would have actually been sent to the machine as the target when the actual positions occurred
self.chainLengths = [
self.computeChainLength(targetPosition, kinematics)
for targetPosition in targetPositions
]
# make sure we have a chain length for each actual position
if len(self.actualPositions) != len(self.chainLengths):
raise ValueError(
'number of target positions and actual positions must match!')
def generateJointTrajectory_now(time, traj_type='circle', tf=2.5):
theta = np.zeros((1, 1))
dt = 0.0001
angles = np.zeros((5, 5))
vel = np.zeros((3, 5))
if traj_type == 'circle':
c_x = 0.0
c_y = 0.3
c_z = 0.
radius = 0.095
for i in range(5):
peturb = dt * float(i - 2)
x, y, z = circle_now(c_x, c_y, c_z, radius, time + peturb, tf)
ang_temp = KMTCS.inverseKinematics(x, y, z, theta)
for j in range(5):
angles[i, j] = ang_temp[j]
for i in range(3):
vel[i] = (angles[:][i + 2] - angles[:][i]) / (2 * dt)
acc = (vel[2] - vel[0]) / (2 * dt)
else:
msg = 'Cannot generate trajectory for unknown type: ' + traj_type
raise ValueError(msg)
return angles[2], vel[1], acc
def __init__(self, client, links, base_offset, x_offset, y_offset, z_offset, min_angles, max_angles, min_servo_pos,
max_servo_pos):
"""
Constructor - used for initialization
move_to_init should be called after initialization to move the robotarm to the initial position.
Servos can only track position after move_to_init has been called! (=High risk of unexpected behaviour!)
:param links: the lengths of all links in an array
:param base_offset: the distance in between the center of the base axis and the first joint
"""
# Params
self.client = client
self.links = links
self.base_offset = base_offset
self.x_offset = x_offset
self.y_offset = y_offset
self.z_offset = z_offset
self.min_angles = min_angles
self.max_angles = max_angles
self.min_servo_pos = min_servo_pos
self.max_servo_pos = max_servo_pos
# Set to initial position - move_to_init should be called after initialization!
# Base, axis 1, axis 2, axis 3, axis 4, grabber
self.servo_pos = [0, 0, 0, 0, 0, 0]
self.kinematics = Kinematics(self.links, self.base_offset, self.x_offset, self.y_offset, self.z_offset)
def plot_TCP(self, axis_poses, column, time_array):
if (len(axis_poses[0]) < 6): return
delta_t = np.divide((time_array[-1] - time_array[0]), len(time_array))
tcp_pos_array = []
for pose in axis_poses:
b = Kinematics.TCPFrame(
dh_param.a_array, dh_param.alfa_array, dh_param.d_array,
[pose[0], pose[1], pose[2], 0, pose[3], pose[4], pose[5]])
tcp_pos = map(lambda p: p[0], np.matrix.tolist(b[0:3, 3]))
tcp_pos_array.append(list(tcp_pos))
x_array = functions.compute_derivative(
self.getColumn(tcp_pos_array, 0), delta_t)
y_array = functions.compute_derivative(
self.getColumn(tcp_pos_array, 1), delta_t)
z_array = functions.compute_derivative(
self.getColumn(tcp_pos_array, 2), delta_t)
tcp_vel_magnitude = list(
map(
lambda x, y, z: np.sqrt(
np.power(x, 2) + np.power(y, 2) + np.power(z, 2)), x_array,
y_array, z_array))
self.ax_array[3][column].plot(time_array[:-1], tcp_vel_magnitude)
self.ax_array[3][column].set_title(text.tcpVelocity)
self.ax_array[3][column].grid()
def __init__(self, position):
"""
position: init position dict with keys:
- "angle": a X-axis relative angle.
- "point": the first position.
"""
self._position = position
# self._us_sensors = RangeSensor(4)
self._motors = Motors(5)
self._kinematic = Kinematics(6)
logging.info('Building the graph map.')
robot_diagonal = math.sqrt(self.DIMENSION['length'] +
self.DIMENSION['width']**2)
self._graph = build_graph(robot_diagonal)
logging.info('Finished to build the graph map.')
def computeJacobianOfResiduals(self, parameters):
''' given a set of solvable parameters, compute the Jacobian matrix of the residuals '''
# cache the number of residual functions
numResidualPairs = self.getNumResidualPairs()
# # set calibration from parameters
# self.setCalibrationFromParameters( parameters, self.calibration )
# loop over each parameter and calculate a column of the jacobian
jacobianColumns = []
for parameterIndex in range(len(parameters)):
# calculate the partial of each residual with respect to this parameter
# make a copy of parameters
parametersCopy = parameters[:]
# offset this parameter to the left by epsilon, construct kinematics, and compute left values for all residuals
parametersCopy[
parameterIndex] = parameters[parameterIndex] - PartialEpsilon
self.setCalibrationFromParameters(parametersCopy, self.calibration)
kinematics = Kinematics(self.calibration)
#leftValueVector = flatten( [ self.computeResidualPair( residualIndex, kin ) for residualIndex in range( numResidualPairs ) ] )
leftValueVector = self.computeVectorOfResiduals(kinematics)
# offset this parameter to the right by epsilon, construct kinematics, and compute right values for all residuals
parametersCopy[
parameterIndex] = parameters[parameterIndex] + PartialEpsilon
self.setCalibrationFromParameters(parametersCopy, self.calibration)
kinematics = Kinematics(self.calibration)
#rightValueVector = flatten( [ self.computeResidualPair( residualIndex, kin ) for residualIndex in range( numResidualPairs ) ] )
rightValueVector = self.computeVectorOfResiduals(kinematics)
# compute slope between left and right values to get jacobian column vector
column = [(rightValue - leftValue) / (2 * PartialEpsilon)
for leftValue, rightValue in zip(leftValueVector,
rightValueVector)]
# append to jacobian columns
jacobianColumns.append(column)
# construct jacobian
jacobian = numpy.array(jacobianColumns).T
# return
return jacobian
def test_round(self):
"""
Tests the round function
"""
print("<Test Round START>")
to_round = [1.8329342, 1.24351622342, 0.2481955, 4.35892392]
rounded = Kinematics.round_results(to_round)
expected = [1.83, 1.24, 0.25, 4.36]
self.assertEqual(rounded, expected)
def generateJointTrajectory(posList, dt):
jointTrajectory = namedtuple('JointTrajectory',
['ang', 'vel', 'acc', 'time'])
theta = np.zeros(posList.x.shape)
jointTrajectory.ang = KMTCS.inverseKinematics(posList.x[0], posList.y[0],
posList.z[0], theta[0])
for i in range(1, posList.x.size):
angles = KMTCS.inverseKinematics(posList.x[i], posList.y[i],
posList.z[i], theta[i])
jointTrajectory.ang = np.append(jointTrajectory.ang, angles, axis=1)
jointTrajectory.vel = np.zeros(jointTrajectory.ang.shape)
jointTrajectory.acc = np.zeros(jointTrajectory.ang.shape)
for i in range(5):
jointTrajectory.vel[i] = differentiate(jointTrajectory.ang[i], dt)
jointTrajectory.acc[i] = differentiate(jointTrajectory.vel[i], dt)
return jointTrajectory
def __init__(self, feature):
self.kinematics = Kinematics(table)
self.qstr = ['q{0}'.format(i + 1) for i in range(
len(self.kinematics.ajoints))]
add_rev = lambda s: feature.addProperty("App::PropertyAngle",
s, "Joint Values", s)
add_prism = lambda s: feature.addProperty("App::PropertyDistance",
s, "Joint Values", s)
for jnt, qstr in zip(self.kinematics.ajoints, self.qstr):
if (jnt.isrevolute()):
add_rev(qstr)
elif (jnt.isprismatic()):
add_prism(qstr)
feature.Proxy = self
def init_all(self, robot):
"""Helper function providing common initialization for initial robot and robot that is set later after editing"""
self.scene.autocenter = True
self.scene.autoscale = True
self.kinematics = Kinematics(robot)
self.assign_scale()
self.init_lists()
self.init_ui()
self.draw()
self.update_spin_controls()
rate(1000)
# rate is executed to perform code above before turning off the autocenter and autoscale
self.scene.autoscale = False
self.scene.autocenter = False
def __init__(self, graspit, robot, body):
default_pose = Pose()
default_pose.position.y = 0.2
default_pose.orientation.w = 1
graspit.clearWorld()
graspit.importRobot(robot)
graspit.setRobotPose(default_pose)
graspit.importGraspableBody(body)
self._graspit = graspit
self._robot = robot
self._body = body
self._kinematics = Kinematics('{}/models/robots/{}'.format(
os.environ['GRASPIT'], robot))
def computeCost(self, calibration):
''' given a new set of calibration parameters, compute the cost function roughly equating to
"how far off are the self.actualPositions from the positions computed from self.chainLengths?"
'''
## cache the number of residual functions
#numResidualPairs = self.getNumResidualPairs()
# create kinematics to calculate positions from chain lengths
kinematics = Kinematics(calibration)
# compute residuals
residuals = self.computeVectorOfResiduals(kinematics)
sqrResiduals = [x * x for x in residuals]
# return sum of the residuals
return sum(sqrResiduals)
def boot_from_dict(cls, data: {}):
"""boot_from_dict
Generates a meArm environment from dictionary
:param data: The dictionary containing the servo data. Must adhere to me_arm.meArmSchema
:type data: dictionary
"""
for c in data:
controller = PCA9685.from_dict(c['controller'])
for a in c['arms']:
level = "INFO"
s = a['servos']
tag = str(s['hip']['channel']).zfill(2) + str(
s['elbow']['channel']).zfill(2) + str(
s['shoulder']['channel']).zfill(2) + str(
s['gripper']['channel']).zfill(2)
id = str(controller.address).zfill(6) + tag
if id in me_arm._instances: continue
if a['logging_level'] is not None: level = a['logging_level']
obj = cls(controller, s['hip']['channel'],
s['elbow']['channel'], s['shoulder']['channel'],
s['gripper']['channel'], False, level)
obj._hip_servo = me_armServo.from_dict(s['hip'])
obj._shoulder_servo = me_armServo.from_dict(s['shoulder'])
obj._elbow_servo = me_armServo.from_dict(s['elbow'])
obj._gripper_servo = me_armServo.from_dict(s['gripper'])
obj._arm_kinematics = me_armKinematics.from_dict(
a['kinematics'])
obj._kinematics = Kinematics(
False, obj._arm_kinematics.humerus,
obj._arm_kinematics.radius,
obj._arm_kinematics.clavicle + obj._arm_kinematics.phalanx)
obj._inc = a['angle-increment']
obj.initialize()
cls._instances[id] = obj
return cls._instances
#!/usr/bin/python
# -*- coding: utf-8 -*-
from math import cos, sin
from numpy import matrix
from numpy import allclose
from numpy import zeros
from numpy import concatenate
from kinematics import Kinematics
from jacobians import serialKinematicJacobian as jacobian
from robots import table_rx90
kin = Kinematics(table_rx90)
kin.set_q([0.1, 0.2, 0.3, 0.4, 0.5, 0.6])
jnts = kin.joints
# This jacobian is obtained from the course 'Modeling and Control of Manipulators' of Wisama Khalil
numer_jac = matrix([[-10.2079, -408.5824, -349.2793, 0, 0, 0],
[101.7389, -40.9950, -35.0448, 0, 0, 0],
[0, 102.2498, -191.7702, 0, 0, 0],
[0, 0.0998, 0.0998, -0.4770, 0.4320, -0.7856],
[0, -0.9950, -0.9950, -0.0479, -0.8823, -0.2665],
[1.0000, 0.0000, 0.0000, 0.8776, 0.1867, 0.5584]
])
J = jacobian(jnts)
assert allclose(numer_jac, J, rtol=8e-04)
# From "Robots-manipulateurs de type série" - Wisama Khalil, Etienne Dombre, p.12.
# http://www.gdr-robotique.org/cours_de_robotique/?id=fd80a49ceaa004030c95cdacb020ec69.
# In French.
q0, q1, q2, q3, q4, q5 = kin.get_q()
from kinematics import Kinematics ik=Kinematics() print ik.getTargetJoints([1,1],[50,0])
SO.append(curve)
x = []
y = []
z = []
for i in range(0, 151):
q = []
for j in range(0, 6):
q.append(SO[j][i])
h = ks.forward(q)
x.append(h[0, 3])
y.append(h[1, 3])
z.append(h[2, 3])
ax.plot(x, y, z, "g")
# ax.plot_wireframe(x, y, z, rstride=10, cstride=10)
plt.show()
# DH parameters
l = 10 # length of link
angle = pi / 2
d = [l, 0, 0, 2 * l, 0, 2 * l]
a = [0, l, 0, 0, 0, 0]
alpha = [angle, 0, angle, -angle, angle, 0]
theta = [0, 0, angle, 0, 0, 0]
ks = Kinematics(a, alpha, d, theta)
#show_manipulation_clouds()
#test_forward_to_inverse()
trajectory_planning()
class SimulationCanvas(GridLayout):
scatterObject = ObjectProperty(None)
bedWidth = 2438.4 #8'
bedHeight = 1219.2 #4'
motorLift = Kinematics.motorOffsetY
motorTranslate = (Kinematics.D - bedWidth) / 2
motorY = bedHeight + motorLift
motor2X = bedWidth + motorTranslate
correctKinematics = Kinematics()
distortedKinematics = Kinematics()
isQuadKinematics = BooleanProperty(True)
def initialize(self):
print "canvas initialized"
self.motorSpacingError.bind(value=self.onSliderChange)
self.motorVerticalError.bind(value=self.onSliderChange)
self.sledMountSpacingError.bind(value=self.onSliderChange)
self.vertBitDist.bind(value=self.onSliderChange)
self.leftChainOffset.bind(value=self.onSliderChange)
self.rightChainOffset.bind(value=self.onSliderChange)
self.rotationRadiusOffset.bind(value=self.onSliderChange)
self.vertCGDist.bind(value=self.onSliderChange)
self.gridSize.bind(value=self.onSliderChange)
Clock.schedule_once(self.moveToCenter, 3)
self.kinematicsSelect.text = "Quadrilateral"
self.recompute()
def moveToCenter(self, *args):
#This moves the simulation onto the screen, I would love if it were centered
#but for now it doesn't adapt to screen size (Window.width, Window.height)
moveVertical = self.bedHeight / 1.4
moveHorizontal = self.bedWidth / 1.4
mat = Matrix().translate(moveHorizontal, moveVertical, 0)
self.scatterInstance.apply_transform(mat)
#scale it down to fit on the screen
self.scatterInstance.apply_transform(Matrix().scale(.3, .3, 1))
def setInitialZoom(self):
mat = Matrix().scale(.4, .4, 1)
self.scatterInstance.apply_transform(mat, (0, 0))
mat = Matrix().translate(200, 100, 0)
self.scatterInstance.apply_transform(mat)
def resetSliders(self):
print "connection made"
self.motorSpacingError.value = 0
self.motorVerticalError.value = 0
self.sledMountSpacingError.value = 0
self.vertBitDist.value = 0
self.vertCGDist.value = 0
self.leftChainOffset.value = 0
self.rightChainOffset.value = 0
self.rotationRadiusOffset.value = 0
self.gridSize.value = 300
def recompute(self):
print "recompute"
#clear the canvas to redraw
self.scatterInstance.canvas.clear()
#re-draw 4x8 outline
self.drawOutline()
leftRigthBound = int(self.correctKinematics.machineWidth / 2)
topBottomBound = int(self.correctKinematics.machineHeight / 2)
self.testPointGenerator = TestPoint()
self.testPointGenerator.initialize(self.scatterInstance.canvas,
self.correctKinematics,
self.distortedKinematics)
self.listOfPointsToPlot = []
self.listOfPointsPlotted = []
self.listOfDistortedPoints = []
self.pointIndex = 0
self.verticalPoints = range(
int(
int(topBottomBound / self.gridSize.value) *
self.gridSize.value), -topBottomBound,
-1 * int(self.gridSize.value))
self.horizontalPoints = range(
int(
int(leftRigthBound / self.gridSize.value) *
self.gridSize.value), -leftRigthBound,
-1 * int(self.gridSize.value))
self.doSpecificCalculation()
for j in self.verticalPoints:
for i in self.horizontalPoints:
point = (i, j)
self.listOfPointsToPlot.append(point)
self.plotNextPoint()
def plotNextPoint(self, *args):
point = self.listOfPointsToPlot[self.pointIndex]
self.pointIndex = self.pointIndex + 1
xValue = point[0]
yValue = point[1]
pointPlotted, distortedPoint = self.testPointGenerator.plotPoint(
xValue, yValue)
self.listOfPointsPlotted.append(pointPlotted)
self.listOfDistortedPoints.append(distortedPoint)
if self.pointIndex < len(self.listOfPointsToPlot):
Clock.schedule_once(self.plotNextPoint)
else:
self.drawLines()
def drawLines(self):
bedWidth = self.correctKinematics.machineWidth
bedHeight = self.correctKinematics.machineHeight
#draw ideal points
for i in range(0, len(self.verticalPoints)):
points = []
for j in range(0, len(self.horizontalPoints)):
point = self.listOfPointsToPlot[j +
i * len(self.horizontalPoints)]
points.append(point[0] + self.bedWidth / 2)
points.append(point[1] + self.bedHeight / 2)
with self.scatterInstance.canvas:
Color(0, 0, 1)
Line(points=points)
for i in range(0, len(self.horizontalPoints)):
points = []
for j in range(0, len(self.listOfPointsToPlot),
len(self.horizontalPoints)):
point = self.listOfPointsToPlot[j + i]
points.append(point[0] + self.bedWidth / 2)
points.append(point[1] + self.bedHeight / 2)
with self.scatterInstance.canvas:
Color(0, 0, 1)
Line(points=points)
#draw distorted points
for i in range(0, len(self.verticalPoints)):
points = []
for j in range(0, len(self.horizontalPoints)):
point = self.listOfDistortedPoints[j + i *
len(self.horizontalPoints)]
points.append(point[0] + self.bedWidth / 2)
points.append(point[1] + self.bedHeight / 2)
with self.scatterInstance.canvas:
Color(1, 0, 0)
Line(points=points)
for i in range(0, len(self.horizontalPoints)):
points = []
for j in range(0, len(self.listOfDistortedPoints),
len(self.horizontalPoints)):
point = self.listOfDistortedPoints[j + i]
points.append(point[0] + self.bedWidth / 2)
points.append(point[1] + self.bedHeight / 2)
with self.scatterInstance.canvas:
Color(1, 0, 0)
Line(points=points)
#draw regular lines
for i in range(0, len(self.verticalPoints)):
points = []
for j in range(0, len(self.horizontalPoints)):
point = self.listOfPointsPlotted[j + i *
len(self.horizontalPoints)]
points.append(point[0] + self.bedWidth / 2)
points.append(point[1] + self.bedHeight / 2)
with self.scatterInstance.canvas:
Color(0, 1, 0)
Line(points=points)
for i in range(0, len(self.horizontalPoints)):
points = []
for j in range(0, len(self.listOfPointsPlotted),
len(self.horizontalPoints)):
point = self.listOfPointsPlotted[j + i]
points.append(point[0] + self.bedWidth / 2)
points.append(point[1] + self.bedHeight / 2)
with self.scatterInstance.canvas:
Color(0, 1, 0)
Line(points=points)
def addPoints(self):
pass
def doSpecificCalculation(self):
print "The horizontal measurement of a centered 48 inch long part cut low down on the sheet is: "
lengthMM = 1219.2
pointPlotted1, distortedPoint1 = self.testPointGenerator.plotPoint(
-lengthMM / 2, -200)
pointPlotted2, distortedPoint2 = self.testPointGenerator.plotPoint(
lengthMM / 2, -200)
print distortedPoint2[0] - distortedPoint1[0]
print "Error MM: " + str(lengthMM -
(distortedPoint2[0] - distortedPoint1[0]))
def setKinematics(self, kinematicsType):
if kinematicsType == "Quadrilateral":
self.isQuadKinematics = True
self.distortedKinematics.isQuadKinematics = True
self.correctKinematics.isQuadKinematics = True
else:
self.isQuadKinematics = False
self.distortedKinematics.isQuadKinematics = False
self.correctKinematics.isQuadKinematics = False
def onSliderChange(self, *args):
self.distortedKinematics.motorOffsetY = self.correctKinematics.motorOffsetY + self.motorVerticalError.value
self.motorVerticalErrorLabel.text = "Motor Vertical\nError: " + str(
int(self.motorVerticalError.value)) + "mm"
self.distortedKinematics.l = self.correctKinematics.l + self.sledMountSpacingError.value
self.sledMountSpacingErrorLabel.text = "Sled Mount\nSpacing Error: " + str(
int(self.sledMountSpacingError.value)) + "mm"
self.distortedKinematics.D = self.correctKinematics.D + self.motorSpacingError.value
self.motorSpacingErrorLabel.text = "Motor Spacing\nError: " + str(
int(self.motorSpacingError.value)) + "mm"
self.distortedKinematics.s = self.correctKinematics.s + self.vertBitDist.value
self.vertBitDistLabel.text = "Vert Dist To\nBit Error: " + str(
int(self.vertBitDist.value)) + "mm"
self.distortedKinematics.h3 = self.correctKinematics.h3 + self.vertCGDist.value
self.vertCGDistLabel.text = "Vert Dist\nBit to CG Error: " + str(
int(self.vertCGDist.value)) + "mm"
self.distortedKinematics.chain1Offset = int(self.leftChainOffset.value)
self.leftChainOffsetLabel.text = "Left Chain\nSlipped Links: " + str(
int(self.leftChainOffset.value)) + "links"
self.distortedKinematics.chain2Offset = int(
self.rightChainOffset.value)
self.rightChainOffsetLabel.text = "Right Chain\nSlipped Links: " + str(
int(self.rightChainOffset.value)) + "links"
self.distortedKinematics.rotationDiskRadius = self.correctKinematics.rotationDiskRadius + self.rotationRadiusOffset.value
self.rotationRadiusLabel.text = "Rotation Radius\nSpacing Error: " + str(
int(self.rotationRadiusOffset.value)) + "mm"
self.machineLabel.text = "distance between sled attachments ideal: " + str(
self.correctKinematics.l
) + " actual: " + str(
self.distortedKinematics.l
) + "mm\nvertical distance between sled attachments and bit ideal: " + str(
self.correctKinematics.s
) + " actual: " + str(
self.distortedKinematics.s
) + "mm\nvertical distance between sled attachments and CG ideal: " + str(
self.correctKinematics.h3 +
self.correctKinematics.s) + " actual: " + str(
self.distortedKinematics.h3 + self.distortedKinematics.s
) + "mm\ndistance between motors ideal: " + str(
self.correctKinematics.D) + " actual: " + str(
self.distortedKinematics.D) + "mm"
self.gridSizeLabel.text = "Grid Size: " + str(int(
self.gridSize.value)) + "mm"
self.distortedKinematics.recomputeGeometry()
def drawOutline(self):
bedWidth = self.correctKinematics.machineWidth
bedHeight = self.correctKinematics.machineHeight
with self.scatterInstance.canvas:
Line(points=(0, 0, 0, bedHeight))
Line(points=(0, bedHeight, bedWidth, bedHeight))
Line(points=(bedWidth, bedHeight, bedWidth, 0))
Line(points=(bedWidth, 0, 0, 0))
def on_touch_up(self, touch):
if touch.is_mouse_scrolling:
self.zoomCanvas(touch)
def zoomCanvas(self, touch):
if touch.is_mouse_scrolling:
scaleFactor = .1
if touch.button == 'scrollup':
mat = Matrix().scale(1 - scaleFactor, 1 - scaleFactor, 1)
self.scatterInstance.apply_transform(mat, anchor=touch.pos)
elif touch.button == 'scrolldown':
mat = Matrix().scale(1 + scaleFactor, 1 + scaleFactor, 1)
self.scatterInstance.apply_transform(mat, anchor=touch.pos)
def setUp(self):
print("<Setup START>")
links = [26, 22, 12.5]
self.kinematics = Kinematics(links, 0, 0, 0, 0)
class Robot:
# US sensors constants.
US_SENSORS = [
{
'name': 'front_bottom',
'trigger_limit': 8,
'sensors': 0
},
{
'name': 'left',
'trigger_limit': 10,
'sensors': 2
},
{
'name': 'back',
'trigger_limit': 8,
'sensors': 4
},
{
'name': 'right',
'trigger_limit': 10,
'sensors': 3
},
{
'name': 'front_top',
'trigger_limit': 8,
'sensors': 1
},
]
# Take in count the obstacle dimension (assuming another robot)
# + the robot size.
OBSTACLES_DIMENSION = 50
def __init__(self):
"""
position: init position dict with keys:
- "angle": a X-axis relative angle.
- "point": the first position.
"""
# self._us_sensors = RangeSensor(4)
self._motors = Motors(5)
self._kinematic = Kinematics(6)
self._us = RangeSensor(4)
logging.info('Building the graph map.')
# self._graph = build_graph(robot_diagonal)
logging.info('Finished to build the graph map.')
self._blocking_servo = -1
def reset_kinematics(self):
self._kinematic.up_clamp()
time.sleep(0.2)
self._kinematic.open_clamp()
time.sleep(0.2)
self._kinematic.reset_funny()
time.sleep(0.2)
self._kinematic.push_back()
time.sleep(1)
def wait_motors(self, enable=True, timeout=0):
first_time = time.time()
while not self._motors.is_done():
if enable:
if self._blocking_servo != -1:
i = self._blocking_servo
test = self._us.get_range(i)
print(test)
if test > self.US_SENSORS[i]['trigger_limit']:
self._motors.restart()
self._blocking_servo = -1
else:
continue
us_sensors = self._us.get_ranges()
print(us_sensors)
for i, us in enumerate(us_sensors):
if 'front' in self.US_SENSORS[i][
'name'] or 'back' in self.US_SENSORS[i]['name']:
if us < self.US_SENSORS[i]['trigger_limit']:
self._motors.stop()
self._blocking_servo = i
break
if (time.time() - first_time) > timeout and timeout != 0:
break
time.sleep(0.1)
def take_modules(self, number=1):
for i in range(number):
self._kinematic.open_clamp()
self._kinematic.down_clamp()
time.sleep(DELAY_UP_DOWN_CLAMP)
self._motors.forward(9)
self.wait_motors(enable=False, timeout=2)
self._kinematic.close_clamp()
time.sleep(1)
# self._motors.set_speed(40)
self._motors.backward(7)
if number == 1:
self._motors.forward(6)
self.wait_motors(enable=False, timeout=2)
self._motors.backward(6)
self.wait_motors(enable=False, timeout=2)
self.wait_motors(enable=False)
self._motors.backward(8)
self.wait_motors(enable=False)
#SEULEMENT SI ON DECIDE DE RECULER POUR MIEUX PRENDRE LE MODULE ---
self._kinematic.open_clamp()
time.sleep(0.5)
self._motors.forward(3)
self.wait_motors(enable=False, timeout=2)
self._kinematic.close_clamp()
time.sleep(1)
#---
self._motors.forward(3)
self._kinematic.up_clamp()
self.wait_motors(enable=False, timeout=3)
time.sleep(0.5)
self._kinematic.open_clamp()
time.sleep(1)
self._kinematic.down_clamp()
time.sleep(0.5)
self._kinematic.up_clamp()
def eject_modules(self, number=1):
for i in range(number):
self._kinematic.push_out()
time.sleep(1.5)
self._kinematic.push_back()
time.sleep(1.5)
self._motors.forward(2)
self.wait_motors(timeout=2)
self._motors.backward(8)
self.wait_motors(timeout=2)
def get_kin(self):
return self._kinematic
def get_motors(self):
return self._motors
def finalize(self):
self._motors.stop()
self._kinematic.launch_funny()
time.sleep(0.8)
self._kinematic.reset_funny()
'r_arm_wrx': 0.0,
'r_arm_wry': 0.0,
'r_arm_wry2': 0.0,
'r_leg_akx': 0.0,
'r_leg_aky': 0.28,
'r_leg_hpx': 0.0,
'r_leg_hpy': 0.13,
'r_leg_hpz': 0.0,
'r_leg_kny': 0.52
}
# Grab the URDF from the parameter server.
urdf = rospy.get_param('/robot_description')
# Set up the kinematics, from pelvis to left and right foot.
ll_kin = Kinematics(urdf, 'pelvis', 'l_foot')
rl_kin = Kinematics(urdf, 'pelvis', 'r_foot')
ll_N = ll_kin.dofs()
rl_N = rl_kin.dofs()
left_leg_joints_names = [
'l_leg_hpz', 'l_leg_hpx', 'l_leg_hpy', 'l_leg_kny', 'l_leg_aky',
'l_leg_akx'
]
left_leg_q = np.array([[joints[n]] for n in left_leg_joints_names])
right_leg_joints_names = [
'r_leg_hpz', 'r_leg_hpx', 'r_leg_hpy', 'r_leg_kny', 'r_leg_aky',
'r_leg_akx'
]
class JointState():
def __init__(self):
#self.cv_image1 = cv_image1
#self.cv_image2 = cv_image2
self.meter_to_pixel_factor = -1
self.initial_angles = [0, 0, 0, 0]
self.time_previous_step = np.array([rospy.get_time()], dtype='float64')
self.error = np.array([0.0, 0.0, 0.0], dtype='float64')
self.error_d = np.array([0.0, 0.0, 0.0], dtype='float64')
self.kinematics = Kinematics()
print("Init JointState")
def compute_joint_state(self, debug=False):
# Detect objects from using images
self.common_image1 = Common_image(self.cv_image1)
self.common_image2 = Common_image(self.cv_image2)
joint_positions1 = self.common_image1.compute_joint_positions()
joint_positions2 = self.common_image2.compute_joint_positions()
if (joint_positions1 is None or joint_positions2 is None):
return None
# if (debug):
# self.common_image1.compute_blob_center(self.common_image1.lower_green, self.common_image1.upper_green, debug=True)
# common_image2.compute_blob_contours(common_image2.lower_green, common_image1.upper_green, debug=True)
# # # # # # # # # # # # # # #
#
# Fix occlusion issues
#
joint_positions1, joint_positions2 = self.handle_occlusion(
joint_positions1, joint_positions2)
# # # # # # # # # # # # # # #
#
# Compute positions
#
self.origin_pos = self.compute_joint_position(
joint_positions1['yellow'], joint_positions2['yellow'])
self.blue_abs_pos = self.compute_joint_position(
joint_positions1['blue'], joint_positions2['blue'])
self.blue_pos = self.transform_to_relative_pos(self.blue_abs_pos,
self.origin_pos)
self.green_abs_pos = self.compute_joint_position(
joint_positions1['green'], joint_positions2['green'])
self.green_pos = self.transform_to_relative_pos(
self.green_abs_pos, self.origin_pos)
self.red_abs_pos = self.compute_joint_position(joint_positions1['red'],
joint_positions2['red'])
self.red_pos = self.transform_to_relative_pos(self.red_abs_pos,
self.origin_pos)
# # # # # # # # # # # # # # #
#
# Compute distances
#
yellow_blue_distance = self.compute_distance_between_joints(
self.origin_pos, self.blue_abs_pos)
blue_green_distance = self.compute_distance_between_joints(
self.blue_abs_pos, self.green_abs_pos)
green_red_distance = self.compute_distance_between_joints(
self.green_abs_pos, self.red_abs_pos)
# # # # # # # # # # # # # # #
#
# Compute pixel to meter factor
#
if (self.meter_to_pixel_factor == -1):
self.meter_to_pixel_factor = self.common_image1.compute_meter_to_pixel_factor(
self.origin_pos, self.blue_abs_pos, 2.0)
if (debug):
print("\nmeter_to_pixel_factor")
print(self.meter_to_pixel_factor)
self.yellow_blue_distance_meters = yellow_blue_distance * self.meter_to_pixel_factor
self.blue_green_distance_meters = blue_green_distance * self.meter_to_pixel_factor
self.green_red_distance_meters = green_red_distance * self.meter_to_pixel_factor
# # # # # # # # # # # # # # #
#
# Compute angles
#
self.joint_angles = self.calculate_angles()
if (debug):
print("\nAngles")
print(self.joint_angles)
return "Done"
def handle_occlusion(self, joint_positions1, joint_positions2):
if (joint_positions1['green'] is None):
print("joint_positions1 occluded")
joint_positions1['green'] = [0, 0]
joint_positions1['green'][0] = joint_positions1['red'][0]
joint_positions1['green'][1] = joint_positions2['green'][1]
if (joint_positions2['green'] is None):
print("joint_positions2 occluded")
joint_positions2['green'] = [0, 0]
joint_positions2['green'][0] = joint_positions2['red'][0]
joint_positions2['green'][1] = joint_positions1['green'][1]
if (joint_positions1['red'] is None):
print("joint_positions1 occluded")
joint_positions1[''] = [0, 0]
joint_positions1['red'][0] = joint_positions1['green'][0]
joint_positions1['red'][1] = joint_positions2['red'][1]
if (joint_positions2['red'] is None):
print("joint_positions2 occluded")
joint_positions2['red'] = [0, 0]
joint_positions2['red'][0] = joint_positions2['green'][0]
joint_positions2['red'][1] = joint_positions1['red'][1]
return joint_positions1, joint_positions2
def compute_joint_position(self, joint_positions1, joint_positions2):
pos_x = joint_positions2[0]
pos_y = joint_positions1[0]
pos_z = (joint_positions1[1] + joint_positions2[1]) / 2
return np.array([pos_x, pos_y, pos_z])
def transform_to_relative_pos(self, pos, origin):
return np.array(
[pos[0] - origin[0], pos[1] - origin[1], origin[2] - pos[2]])
def compute_distance_between_joints(self, joint1_pos, joint2_pos):
return norm(joint1_pos - joint2_pos)
def calculate_angles(self):
# Uses least squares to compute joint angles.
# The residuals function uses the green and red positions computed using opencv and
# compares it to the forward kinematics.
# Since we have 4 angles we need 4 differences, we are using green_x, green_y, green_z and red_x
def f(transient_angles):
t1 = self.kinematics.transform(self.yellow_blue_distance_meters,
transient_angles[0] - np.pi / 2, 0,
-np.pi / 2)
t2 = self.kinematics.transform(0, transient_angles[1] - np.pi / 2,
0, np.pi / 2)
t3 = self.kinematics.transform(0, transient_angles[2],
self.blue_green_distance_meters,
-np.pi / 2)
t4 = self.kinematics.transform(0, transient_angles[3],
self.green_red_distance_meters, 0)
green_kinematics = t1.dot(t2).dot(
t3) # Kinematics up to green joint
green_kinematics_x = green_kinematics[0, 3]
green_kinematics_y = green_kinematics[1, 3]
green_kinematics_z = green_kinematics[2, 3]
red_kinematics = green_kinematics.dot(
t4) # Kinematics up to red joint
red_kinematics_x = red_kinematics[0, 3]
red_kinematics_y = red_kinematics[1, 3]
red_kinematics_z = red_kinematics[2, 3]
return np.array([
self.green_pos[0] * self.meter_to_pixel_factor -
green_kinematics_x,
self.green_pos[1] * self.meter_to_pixel_factor -
green_kinematics_y,
self.green_pos[2] * self.meter_to_pixel_factor -
green_kinematics_z,
self.red_pos[0] * self.meter_to_pixel_factor -
red_kinematics_x,
self.red_pos[1] * self.meter_to_pixel_factor -
red_kinematics_y,
self.red_pos[2] * self.meter_to_pixel_factor - red_kinematics_z
])
bounds = (np.array([-np.pi / 2, -np.pi / 2, -np.pi / 2, -np.pi / 2]),
np.array([np.pi / 2, np.pi / 2, np.pi / 2, np.pi / 2]))
angles = least_squares(f,
self.initial_angles,
jac="3-point",
loss="cauchy",
bounds=bounds)
self.theta1 = angles.x[0]
self.theta2 = angles.x[1]
self.theta3 = angles.x[2]
self.theta4 = angles.x[3]
return angles.x
def estimated_angles(self):
return np.array([self.theta1, self.theta2, self.theta3, self.theta4])
def compute_target_position(self, debug):
target_position1 = self.common_image1.compute_target_center()
target_position2 = self.common_image2.compute_target_center()
if (target_position1 is None or target_position2 is None):
return None
self.target_abs_pos = self.compute_joint_position(
target_position1, target_position2)
self.target_pos = self.transform_to_relative_pos(
self.target_abs_pos, self.origin_pos)
if (debug):
print("\nTarget position")
print(self.target_pos)
# self.common_image1.compute_blob_center(self.common_image1.lower_target, self.common_image1.upper_target, debug=True)
# self.common_image2.compute_blob_center(self.common_image2.lower_target, self.common_image2.upper_target,
# debug=True)
cv2.circle(
self.cv_image1,
(int(self.target_abs_pos[1]), int(self.target_abs_pos[2])),
5, (90, 10, 205),
thickness=2,
lineType=8)
cv2.circle(
self.cv_image2,
(int(self.target_abs_pos[0]), int(self.target_abs_pos[2])),
5, (90, 10, 205),
thickness=2,
lineType=8)
# cv2.imshow('Target Y-Z', self.cv_image1)
# cv2.imshow('Target X-Z', self.cv_image2)
# cv2.waitKey(1)
# Target position correction
return self.target_pos * np.array([1.07, 0.841, 1.07]) + np.array(
[0, 0, 13])
def control_closed(self, debug=False):
# P gain
K_p = np.array([[1, 0, 0], [0, 1, 0], [0, 0, 8]])
# D gain
K_d = np.array([[0.1, 0, 0], [0, 0.1, 0], [0, 0, 0.1]])
# estimate time step
cur_time = np.array([rospy.get_time()])
dt = cur_time - self.time_previous_step
# if (debug):
# print("\ncontrol_closed")
# print("dt")
# print(dt)
# if (dt<0.1):
# return
#print(dt)
self.time_previous_step = cur_time
# robot end-effector position
red_pos = self.red_pos * self.meter_to_pixel_factor
# desired position
target_pos_d = self.target_pos * self.meter_to_pixel_factor
# estimate derivative of error
self.error_d = ((target_pos_d - red_pos) - self.error) / dt
print(self.error)
# estimate error
self.error = target_pos_d - red_pos
jacobian = self.kinematics.jacobian(self)
J_inv = np.linalg.pinv(
jacobian) # calculating the pseudo inverse of Jacobian
dq_d = np.dot(J_inv, (np.dot(K_d, self.error_d.transpose()) +
np.dot(K_p, self.error.transpose())
)) # control input (angular velocity of joints)
# if (debug):
# print(jacobian)
# print(dq_d)
# print(self.error)
q_d = self.estimated_angles() + (
dt * dq_d) # control input (angular position of joints)
return q_d
#!/usr/bin/python # -*- coding: utf-8 -*- from kinematics import Kinematics from robots import table1 from robots import table_sr400 kin = Kinematics(table1) kin.set_q([1, 1, 1]) assert (kin.ajoints[0].q == 1) from kinematics import get_looproot kin = Kinematics(table_sr400) jnts = kin.joints assert (get_looproot(jnts, jnts[9]) == jnts[0]) from kinematics import get_loops kin = Kinematics(table_sr400) jnts = kin.joints assert (get_loops(jnts) == [(jnts[0], jnts[8])])
#!/usr/bin/python # -*- coding: utf-8 -*- from kinematics import Kinematics from robots import table1 from robots import table_sr400 kin = Kinematics(table1) kin.set_q([1, 1, 1]) assert(kin.ajoints[0].q == 1) from kinematics import get_looproot kin = Kinematics(table_sr400) jnts = kin.joints assert(get_looproot(jnts, jnts[9]) == jnts[0]) from kinematics import get_loops kin = Kinematics(table_sr400) jnts = kin.joints assert(get_loops(jnts) == [(jnts[0], jnts[8])])
class Robotarm:
"""
Main controller-class for the 00SIRIS Robotarm
Hedgehog information:
-2000 Steps per Servo
"""
def __init__(self, client, links, base_offset, x_offset, y_offset, z_offset, min_angles, max_angles, min_servo_pos,
max_servo_pos):
"""
Constructor - used for initialization
move_to_init should be called after initialization to move the robotarm to the initial position.
Servos can only track position after move_to_init has been called! (=High risk of unexpected behaviour!)
:param links: the lengths of all links in an array
:param base_offset: the distance in between the center of the base axis and the first joint
"""
# Params
self.client = client
self.links = links
self.base_offset = base_offset
self.x_offset = x_offset
self.y_offset = y_offset
self.z_offset = z_offset
self.min_angles = min_angles
self.max_angles = max_angles
self.min_servo_pos = min_servo_pos
self.max_servo_pos = max_servo_pos
# Set to initial position - move_to_init should be called after initialization!
# Base, axis 1, axis 2, axis 3, axis 4, grabber
self.servo_pos = [0, 0, 0, 0, 0, 0]
self.kinematics = Kinematics(self.links, self.base_offset, self.x_offset, self.y_offset, self.z_offset)
def move_to_init(self):
"""
Moves the robotarm to the initial position
"""
print("Moving to init!")
self.move_to_config([0.0, 1.51, -1.51, 0.0, 0.0, 0.0])
print("Successfully moved to init!")
def move_to_pos(self, pos, joint):
"""
Moves the given joint to the given position
:param pos: the position
:param joint: the joint that will be used
:raise OutOfReachError: if the given position cannot be reached
"""
if 0 <= pos <= 2000: # TODO: maybe this can be changed to servo_min/max but this has to be checked properly
self.client.set_servo(joint, True, int(pos))
self.servo_pos[joint] = pos
else:
raise OutOfReachError("Position out of reach! (Range 0 to 2000): " + str(pos))
def move_to_pos_t(self, pos, joint, duration):
"""
Moves a joint to the given position in exactly the given duration
:param pos: the position that the servo should move to
:param joint: the joint that should be moved
:param duration: the duration that the movement should take
"""
start_position = self.servo_pos[joint]
movement = pos - start_position
print("Starting!")
if movement == 0:
print("Already at position")
return # Already at position
print("Starting procedure!")
time_per_step = abs(duration / movement)
start_time = time.time()
while start_time + duration > time.time() and pos != self.servo_pos[joint]:
crnt_step = round((time.time() - start_time) / time_per_step)
self.move_to_pos(start_position + crnt_step * math.copysign(1, movement), joint)
time.sleep(0.001) # To prevent from overload
def move_to_multi_pos_t(self, positions, duration):
"""
Moves the joints to the given position in exactly the given duration.
If less than 6 positions are given, the first x joints are moved, where x is the number of given positions
:param positions: the positions that the servos should move to
:param duration: the duration that the movement should take
"""
if len(positions) < 1 or len(positions) > 6:
raise OutOfReachError("1-6 positions required!")
start_positions = self.servo_pos
movements = [pos - self.servo_pos[joint] for joint, pos in enumerate(positions)]
# If movement = 0 -> Don't move: Set times_per_step to 0 and check for 0 when calculating crnt step
times_per_step = [abs(duration / movement) if movement != 0 else 0 for movement in movements]
start_time = time.time()
while start_time + duration > time.time():
# If time_per_step = 0 -> Movement = 0 -> Stay at position
crnt_steps = [
round((time.time() - start_time) / time_per_step) if time_per_step != 0 else 0 for
joint, time_per_step in enumerate(times_per_step)]
cfg = [(joint, True, int(start_positions[joint] + crnt_step * math.copysign(1, movements[joint]))) for
joint, crnt_step in enumerate(crnt_steps)]
# TODO: Just using the first 4 axis for now([:4]). Needs a 2nd hedgehog for more
self.client.set_multi_servo(cfg[:4])
time.sleep(0.001) # To prevent from overload
# Update positions
for index in range(len(positions), 6):
positions.append(self.servo_pos[index])
self.servo_pos = positions
def move_to_angle(self, angle, joint):
"""
Moves a joint to a certain angle
:param angle: the angle
:param joint: the joint
"""
pos = self.angle_to_step(angle, joint)
self.move_to_pos(pos, joint)
def move_to_cartesian(self, x, y, z, fixed_joint, fj_angle):
"""
Moves the robotarm to the given cartesian coordinates.
:param x: the x-coordinate
:param y: the y-coordinate
:param z: the z-coordinate
:param fixed_joint: the joint that is fixed
:param fj_angle: the angle of the fixed joint
"""
try:
angles = self.kinematics.inverse(x, y, z, fixed_joint, fj_angle)
self.move_to_config(angles)
except KinematicsError as ke:
print("Position cannot be reached: " + ke.message)
except OutOfReachError as oore:
print("The kinematics module was able to calculate a position, but the servos are not able to reach it: " +
oore.message)
def move_to_cartesian_t(self, x, y, z, fixed_joint, fj_angle, duration):
"""
Moves the robotarm to the given cartesian coordinates.
:param x: the x-coordinate
:param y: the y-coordinate
:param z: the z-coordinate
:param fixed_joint: the joint that is fixed
:param fj_angle: the angle of the fixed joint
:param duration: the duration of the movement
"""
try:
angles = self.kinematics.inverse(x, y, z, fixed_joint, fj_angle)
self.move_to_config_t(angles, duration)
except KinematicsError as ke:
print("Position cannot be reached: " + ke.message)
except OutOfReachError as oore:
print("The kinematics module was able to calculate a position, but the servos are not able to reach it: " +
oore.message)
def move_to_cartesian_aligned(self, x, y, z, alignment):
"""
Moves the robotarm to the given cartesian coordinates.
This one takes the alignment of the grabber towards the x-axis and uses it as the fixed joint.
:param x: the x-coordinate
:param y: the y-coordinate
:param z: the z-coordinate
:param alignment: the alignment of the grabber
"""
try:
angles = self.kinematics.inverse_aligned(x, y, z, alignment)
self.move_to_config(angles)
except KinematicsError as ke:
print("Position cannot be reached: " + ke.message)
except OutOfReachError as oore:
print("The kinematics module was able to calculate a position, but the servos are not able to reach it: " +
oore.message)
def move_to_cartesian_aligned_t(self, x, y, z, alignment, duration):
"""
Moves the robotarm to the given cartesian coordinates.
This one takes the alignment of the grabber towards the x-axis and uses it as the fixed joint.
:param x: the x-coordinate
:param y: the y-coordinate
:param z: the z-coordinate
:param alignment: the alignment of the grabber
:param duration: the duration of the movement
"""
try:
angles = self.kinematics.inverse_aligned(x, y, z, alignment)
self.move_to_config_t(angles, duration)
except KinematicsError as ke:
print("Position cannot be reached: " + ke.message)
except OutOfReachError as oore:
print("The kinematics module was able to calculate a position, but the servos are not able to reach it: " +
oore.message)
def move_to_config(self, angles):
"""
Move the robotarm to the given configuration.
:param angles: the angles of all joints
"""
if self.validate_config(angles):
positions = [self.angle_to_step(angle, joint) for joint, angle in enumerate(angles)]
cfg = [(joint, True, position) for joint, position in enumerate(positions)]
# TODO: Just using the first 4 axis for now([:4]). Needs a 2nd hedgehog for more
self.client.set_multi_servo(cfg[:4])
# Update positions
for index in range(len(positions), 6):
positions.append(self.servo_pos[index])
self.servo_pos = positions
else:
raise OutOfReachError("The given configuration cannot be reached!")
def move_to_config_t(self, angles, duration):
"""
Move the robotarm to the given configuration in the given time.
:param angles: the angles of all joints
:param duration: the duration of the movement
"""
if self.validate_config(angles):
positions = [self.angle_to_step(angle, joint) for joint, angle in enumerate(angles)]
self.move_to_multi_pos_t(positions, duration) # Already updates the servo positions
else:
raise OutOfReachError("The given configuration cannot be reached!")
def move_through_configs_t(self, configs, duration):
"""
Moves the joints through all the given configurations in the given duration
If less than 6 positions are given, the first x joints are moved, where x is the number of given positions
:param configs: the configurations as list of list of angles
e.g. [[0.0, 1.7, -2.0, 0.3], [0.0, 1.7, -2.1, 0.5], ...]
:param duration: the duration that the movement should take
"""
for index, config in enumerate(configs):
if len(config) < 1 or len(config) > 6:
raise OutOfReachError("1-6 positions required!")
# Pre-validation, to find errors BEFORE the movement starts and not in between:
if not self.validate_config(config):
raise OutOfReachError("At least one of the given angles is not valid! Index: " + str(index))
time_per_config = duration/len(configs)
start_time = time.time()
while start_time + duration > time.time():
crnt_step = int((time.time() - start_time) / time_per_config)
# Using move_to_config_t to make the movement "smoother"
# To leave some room for calculation-time, only 90% of the intended time is used as parameter
self.move_to_config_t(configs[crnt_step], time_per_config * 0.9)
time.sleep(0.001) # To prevent from overload
def move_linear_aligned_t(self, a, b, align_start, align_end, resolution, duration):
"""
Moves from point a to point b in a given time.
:param a: the point a as (x,y,z)-tuple
:param b: the point b as (x,y,z)-tuple
:param align_start: the alignment of the grabber at the start of the movement
:param align_end: the alignment of the grabber at the start of the movement
:param resolution: the number of points that will be used for the movement
:param duration: the duration of the movement
"""
points = self.kinematics.linear(a, b, resolution)
angles = self.kinematics.linear_aligned(points, align_start, align_end)
self.move_through_configs_t(angles, duration)
def angle_to_step(self, angle, joint):
"""
Calculates the step value of the given angle for the given joint.
Servos have 2048 steps and this maps the angle to those
:param angle: the angle in radiant representation
:param joint: the joint that should be used
:return: the servo-position in steps
"""
if not self.validate_angle(angle, joint):
raise OutOfReachError
if self.min_angles[joint] == self.max_angles[joint]:
return 0 # if no min/max angle is set for the servo, position does not matter
total_steps = self.max_servo_pos[joint] - self.min_servo_pos[joint]
total_angle = self.max_angles[joint] - self.min_angles[joint]
"""
>Move angle to 0 ("remove" min)
>Calculate steps/angle ratio
>Add min servo pos (offset)
"""
pos = int((total_steps / total_angle) * (angle - self.min_angles[joint]) + self.min_servo_pos[joint])
return pos
def step_to_angle(self, pos, joint):
"""
Calculates the angle of the given servo-position (in steps) for the given joint.
Servos have 2048 steps and this maps the angle to those
:param pos: the given step
:param joint: the joint that should be used
:return: the servo-position in angle
"""
# TODO: Reimplement this!
return 0
def validate_config(self, angles):
"""
Validates whether the given configuration is reachable.
:param angles: the angles of all joints
:return: True if the given configuration is reachable, else: False
"""
if len(angles) < 4:
return False # the K-00SIRIS needs at least 4 joints for a kinematic configuration
# Check whether the angles on all joints can be reached
for joint, angle in enumerate(angles):
if not self.validate_angle(angle, joint):
return False
return True
def validate_angle(self, angle, joint):
"""
Validates whether the given angle is reachable with the given joint.
:param angle: the angle that should be validated
:param joint: the joint that the angle should be validated for
:return: True if the given angle is reachable with the given joint, else: False
"""
if self.min_angles[joint] <= angle <= self.max_angles[joint] or self.min_angles[joint] >= angle >= \
self.max_angles[joint]:
return True
else:
return False
def get_tool_cs(self):
"""
Returns the tool coordinate system
:return: the tool coordinate system as WorldCoordinateSystem
"""
angles = []
for joint, pos in enumerate(self.servo_pos):
angles[joint] = self.step_to_angle(pos, joint)
x, y, z, theta_x, theta_y, theta_z = self.kinematics.direct(angles)
return WorldCoordinateSystem(x, y, z, theta_x, theta_y, theta_z)
def servos_off(self):
"""
Turns off all servos
"""
self.client.set_multi_servo([(0, False, 0), (1, False, 0), (2, False, 0), (3, False, 0)])
# from platform_input_tk import InputInterface # tkinter gui
# from platform_input import InputInterface # keyboard
# from platform_input_simple_UDP import InputInterface # UDP
# from platform_input_threadedUDP import InputInterface # threaded UDP
# from coaster_client import InputInterface
from kinematics import Kinematics
from shape import Shape
from platform_output import OutputInterface
isActive = True # set False to terminate
frameRate = 0.05
client = InputInterface()
chair = OutputInterface()
shape = Shape(frameRate)
k = Kinematics()
class Controller:
def __init__(self):
self.prevT = 0
self.is_output_enabled = False
geometry = chair.get_geometry()
k.set_geometry(geometry[0], geometry[1], geometry[2])
limits = chair.get_limits()
shape.begin(limits, "shape.cfg")
def init_gui(self, root):
self.root = root
self.root.geometry("620x360")
if os.name == 'nt':
class Graphics:
def init_all(self, robot):
"""Helper function providing common initialization for initial robot and robot that is set later after editing"""
self.scene.autocenter = True
self.scene.autoscale = True
self.kinematics = Kinematics(robot)
self.assign_scale()
self.init_lists()
self.init_ui()
self.draw()
self.update_spin_controls()
rate(1000)
# rate is executed to perform code above before turning off the autocenter and autoscale
self.scene.autoscale = False
self.scene.autocenter = False
def __init__(self, panel, sizer, change_function, robot=table, width=600, height=600, x=900, y=0):
"""Arguments:
panel - wxPanel on UI-elements will be placed
sizer - wxBoxSizer (to make proper layout)
change_function - a pointer to the function which is called
when a particular joint is selected in order to show its geometrical parameters
robot - a list or tuple of tuples representing each row of the robot description table
width, height, x, y - self-explanatory
"""
self.p = panel
self.main_sizer = sizer
self.scene = c_display(x=x, y=y, width=width, height=height, forward=vector(0,1,0), background=(1,1,1), title="Simulation")
self.scene.bind('mousedown', self.grab)
self.scene.bind('click', self.select)
distant_light(direction=(0, -1, 0), color=color.gray(0.35))
# cost_tolerance defines the value of tolerance required for convergence of loops
self.cost_tolerance = 20
self.init_all(robot)
# Reference to the function which is called when a joint is selected
self.ch_func = change_function
def set_robot(self, robot=table):
""" The function is called when the graphics window is already displayed to draw new robot description """
for obj in self.scene.objects:
obj.visible = False
del obj
self.init_all(robot)
def init_ui(self):
"""Initialization of UI-elements"""
top_sizer = wx.BoxSizer(wx.HORIZONTAL)
gridControl = wx.GridBagSizer(hgap=10, vgap=10)
cb = wx.CheckBox(self.p, label="Exploded View")
cb.SetValue(True)
cb.Bind(wx.EVT_CHECKBOX, self.changeRepresentation)
gridControl.Add(cb, pos=(0,0), flag=wx.ALIGN_CENTER_VERTICAL)
self.cb_end_effs = wx.CheckBox(self.p, label="End-effectors")
self.cb_end_effs.SetValue(True)
self.cb_end_effs.Bind(wx.EVT_CHECKBOX, self.end_effector_change)
gridControl.Add(self.cb_end_effs, pos=(1,0), flag=wx.ALIGN_CENTER_VERTICAL)
cb_world = wx.CheckBox(self.p, label="Base frame")
cb_world.SetValue(False)
cb_world.Bind(wx.EVT_CHECKBOX, self.world_frame)
gridControl.Add(cb_world, pos=(2,0), flag=wx.ALIGN_CENTER_VERTICAL)
self.tButton = wx.ToggleButton(self.p, label="All Frames")
self.tButton.SetValue(True)
self.tButton.Bind(wx.EVT_TOGGLEBUTTON, self.show_all_frames)
gridControl.Add(self.tButton, pos=(3,0), flag=wx.ALIGN_CENTER)
self.btnReset = wx.Button(self.p, label="Reset All")
self.btnReset.Bind(wx.EVT_BUTTON, self.reset_joints)
gridControl.Add(self.btnReset, pos=(7,0), flag=wx.ALIGN_CENTER)
btnRandom = wx.Button(self.p, label="Random")
btnRandom.Bind(wx.EVT_BUTTON, self.find_random)
gridControl.Add(btnRandom, pos=(8,0), flag=wx.ALIGN_CENTER)
self.solve_loops = self.kinematics.contains_loops()
self.contains_loops = self.solve_loops
if self.kinematics.contains_loops():
self.radioBox = wx.RadioBox(self.p, choices = ['Make Loops', 'Break Loops'], style=wx.RA_SPECIFY_ROWS)
self.radioBox.Bind(wx.EVT_RADIOBOX, self.select_loops)
gridControl.Add(self.radioBox, pos=(9,0), flag=wx.ALIGN_CENTER)
self.lblConvergence = wx.StaticText(self.p, label=' Convergence ')
self.lblConvergence.SetForegroundColour((0,127,0))
font = self.lblConvergence.GetFont()
font.SetWeight(wx.BOLD)
self.lblConvergence.SetFont(font)
gridControl.Add(self.lblConvergence, pos=(10,0), flag=wx.ALIGN_CENTER)
self.btnActive = wx.Button(self.p, label="Solve Active")
self.btnActive.Bind(wx.EVT_BUTTON, self.find_close)
gridControl.Add(self.btnActive, pos=(11,0), flag=wx.ALIGN_CENTER)
self.btnReset.SetLabelText("Reset Active")
self.opacity = 1.
s = wx.Slider(self.p, size=(100,20), minValue=0, maxValue=99, value=99)
s.Bind(wx.EVT_SCROLL, self.change_opacity)
label = wx.StaticText(self.p, label='Joint opacity')
gridControl.Add(label, pos=(5,0), flag=wx.ALIGN_CENTER)
gridControl.Add(s, pos=(6,0), flag=wx.ALIGN_CENTER)
self.spin_ctrls = []
gridJoints = wx.GridBagSizer(hgap=10, vgap=10)
for i, jnt in enumerate(self.kinematics.mjoints):
gridJoints.Add(wx.StaticText(self.p, label='q'+str(jnt.j)), pos=(i,0), flag=wx.ALIGN_CENTER_VERTICAL)
if jnt.isprismatic():
s = wx.SpinCtrlDouble(self.p, size=(70,-1), id=i, min=0, max=self.max_prismatic, value="0")
else:
s = wx.SpinCtrlDouble(self.p, size=(70,-1), id=i, min=-180, max=180, value="0")
s.Bind(wx.EVT_SPINCTRLDOUBLE, self.setq)
s.SetDigits(2)
if jnt.ispassive():
s.Enable(False)
self.spin_ctrls.append(s)
s.SetIncrement(5)
gridJoints.Add(s, pos=(i,1), flag=wx.ALIGN_CENTER_VERTICAL)
choices = []
for i, jnt in enumerate(self.kinematics.joints):
choices.append("Frame " + str(jnt.j))
self.drag_pos = None
self.check_list = wx.CheckListBox(self.p, choices=choices)#, size=wx.Size(100, len(choices)*(cb.GetSize().GetHeight()+1)))
self.check_list.SetChecked(range(len(choices)))
self.check_list.Bind(wx.EVT_CHECKLISTBOX, self.check_frames)
gridControl.Add(self.check_list, pos=(4,0), flag=wx.ALIGN_CENTER)
top_sizer.Add(gridJoints, 0, wx.ALL, 10)
top_sizer.AddSpacer(10)
top_sizer.Add(gridControl, 0, wx.ALL, 10)
self.main_sizer.Add(top_sizer, 0, wx.ALL, 12)
if __name__ == "__main__":
self.p.SetSizerAndFit(self.main_sizer)
def show_frames(self, list):
"""Diplays frames which are in the list """
selected_frames = [f for i, f in enumerate(self.frames) if i in list]
for i, obj in enumerate(self.frames_arrows):
obj.visible = True if obj.frame in selected_frames else False
def init_lists(self):
"""This function is called whenever the objects in the display are redrawn.
"""
self.objs = []
self.frames_arrows = []
self.end_frames = []
self.frames = []
# cylinders and next joint
self.prismatic_links = []
def show_end_effectors(self, show=True):
"""Draws or removes end-effectors (Which are just joints that are not antecedents of other joints and are not cut joints
"""
if show:
indices = self.kinematics.get_last_joints_indices()
for index in indices:
obj = self.objs[index]
direction = self.direction(self.kinematics.joints[index])
eframe = frame(frame=obj.frame, axis=(direction, 0, 0))
self.end_frames.append(eframe)
cylinder(frame=eframe, axis=(self.len_obj/4, 0, 0), radius=self.rad_con, opacity=0.5, color=color.magenta)
cylinder(frame=eframe, axis=(0, self.len_obj/6, 0), radius=self.rad_con, opacity=0.5, pos=(self.len_obj/4, 0, 0), color=color.magenta)
cylinder(frame=eframe, axis=(0, -self.len_obj/6, 0), radius=self.rad_con, opacity=0.5, pos=(self.len_obj/4, 0, 0), color=color.magenta)
cylinder(frame=eframe, axis=(self.len_obj/6, 0, 0), radius=self.rad_con, opacity=0.5, pos=(self.len_obj/4, self.len_obj/6, 0), color=color.magenta)
cylinder(frame=eframe, axis=(self.len_obj/6, 0, 0), radius=self.rad_con, opacity=0.5, pos=(self.len_obj/4, -self.len_obj/6, 0), color=color.magenta)
if isinstance(obj, cylinder):
eframe.pos = (obj.pos.x + self.len_obj/2 + direction*self.len_obj/2, 0, 0)
else:
eframe.pos = (obj.pos.x + direction*self.len_obj/2, 0, 0)
else:
for f in self.end_frames:
for obj in f.objects:
obj.visible = False
del obj
self.end_frames = []
def put_frame(self, frame, i=0):
""" Helper function to draw the z and x - axes with the labels on them """
frame.visible = True
visible = self.check_list.IsChecked(i)
self.frames_arrows.append(arrow(frame=frame, axis=(self.len_obj,0,0), color=color.red, shaftwidth=self.len_obj/25., visible=visible))
self.frames_arrows.append(label(frame=frame, text="z"+str(i+1), pos=(self.len_obj,0), opacity=0, yoffset=3, color=color.black, border=0, box=False, line=False, visible=visible))
self.frames_arrows.append(arrow(frame=frame, axis=(0,self.len_obj,0), color=color.green, shaftwidth=self.len_obj/25., visible=visible))
self.frames_arrows.append(label(frame=frame, text="x"+str(i+1), pos=(0,self.len_obj), opacity=0, yoffset=3, color=color.black, border=0, box=False, line=False, visible=visible))
def assign_scale(self):
""" This function calculates coefficients which are used to draw the objects (Joints, links, end-effectors)
It computes the minimum and maximum r or d different from 0.
Then uses those sizes to determine the reference numbers which are used all over the class.
self.len_obj determines the length of prismatic and revolute joints. In addition, it is used as reference with constant coefficient to determine other sizes and lengths
self.rad_con determines the radius of links
self.max_prismatic defines the upper limit for prismatic joints [0; self.max_prismatic]
"""
minv = inf
maxv = 0
for i, jnt in enumerate(self.kinematics.joints):
dist = max(abs(jnt.r), abs(jnt.d))
if dist < minv and dist != 0:
minv = dist
if dist > max:
maxv = dist
if minv == inf:
minv, maxv = 100, 100
self.len_obj = minv/2.
self.rad_con = self.len_obj/50.
self.max_prismatic = 4*(minv + maxv)
self.kinematics.set_ub(self.max_prismatic)
def direction(self, jnt):
"""Returns 1 if the direction of previous r was positive otherwise 0
It is used to determine the direction of shifts in expanded view.
(Scientific Representation close to Wisama Khalil's representation in the books)
"""
while jnt:
if jnt.r != 0:
return jnt.r/abs(jnt.r)
jnt = jnt.antc
return 1
def check_cost(self, cost):
""" Determines if the found solution is tolerable. If not, displays "No Convergence"
"""
if self.contains_loops:
if (cost > self.cost_tolerance):
self.lblConvergence.SetLabelText("No Convergence!")
self.lblConvergence.SetForegroundColour((255,0,0))
self.btnActive.Enable(True)
return False
else:
self.lblConvergence.SetLabelText(" Convergence ")
self.lblConvergence.SetForegroundColour((0,127,0))
self.btnActive.Enable(False)
return True
def draw(self, scientific=True):
"""
The core method of this class. The function draws all the objects once, assigns to corresponding frames.
The next time redraw-method is called to simplify calculations and drawing by changing depending on the frame positions and orientations.
This method makes use of DGM transformation matrices to calculate frames and links.
"""
transforms, cost = self.kinematics.get_joint_transforms(self.solve_loops)
self.check_cost(cost)
shiftp = 0
for i, jnt in enumerate(self.kinematics.joints):
T = transforms[i]
f = frame(axis=T[:3,2],pos=T[:3,3], up=T[:3,0])
self.frames.append(f)
if jnt.antc:
prevIndex = self.kinematics.joints.index(jnt.antc)
prevF = self.frames[prevIndex]
# To have nice and consistent representation and to apply expanded representation
# One vector (link) connecting 2 consecutive frames is broken down into 2 vectors:
p = vector(jnt.T[2,3], jnt.T[0,3], jnt.T[1,3])
a = vector(jnt.T[2,2], jnt.T[0,2], jnt.T[1,2])*jnt.r + vector(1,0,0)*jnt.b
q = p - a
# The first one is along x or u of the first frame
cylinder(frame=prevF, axis=q, radius=self.rad_con, opacity=0.5)
# The second one is along z of the second frame
cyl = cylinder(frame=f, axis=(-jnt.r, 0, 0), radius=self.rad_con, opacity=0.5)
if jnt.isprismatic():
# The second one is added into prismatic links list
# because it may change not only position but length as well
self.prismatic_links.append((cyl, jnt))
if jnt.isprismatic():
obj = box(frame=f, axis=(self.len_obj,0,0), height=self.len_obj/4., width=self.len_obj/4., opacity=self.opacity, color= color.orange)
elif jnt.isrevolute():
obj = cylinder(frame=f, pos=(-self.len_obj/2,0,0), axis=(self.len_obj,0,0), radius=self.len_obj/8., opacity=self.opacity, color=color.yellow)
else:
obj = sphere(frame=f, radius=self.len_obj/6., color=color.green, opacity=self.opacity)
self.put_frame(f, i)
self.objs.append(obj)
# If representation is exploded (scientific) and 2 consecutive frames have the same position
if scientific and jnt.d == 0 and jnt.r == 0:
# Shift If:
# the previous frame was the first
if i == 1:
self.objs[i-1].pos -= (3*self.len_obj/2,0,0)
cylinder(frame=prevF, axis=(-3*self.len_obj/2, 0, 0), radius=self.rad_con, opacity=0.5)
# or current frame is the last
elif self.kinematics.is_last_joint(jnt):
diff = vector(self.direction(jnt.antc)*3*self.len_obj/2,0,0)
obj.pos += diff
cylinder(frame=f, axis=diff, radius=self.rad_con, opacity=0.5)
# or previous frame had r != 0
elif jnt.antc and jnt.antc.r != 0:
# previous was coaxial prismatic with r != 0
if not jnt.antc.isrevolute() and prevF.axis == f.axis:
shiftp = (abs(jnt.antc.r) - 2*self.len_obj)/3
self.objs[prevIndex].pos -= vector(2*shiftp + 3* self.len_obj/2,0,0)*self.direction(jnt.antc)
# previous was revolute with r != 0
else:
self.objs[prevIndex].pos -= vector(jnt.antc.r/2,0,0)
# or the one before previous was prismatic
elif shiftp:
self.objs[prevIndex].pos -= vector(shiftp + self.len_obj/2,0,0)*self.direction(jnt.antc)
shiftp = 0
self.show_end_effectors(self.cb_end_effs.Value)
def redraw(self):
"""This function is used to reposition the frames. All other objects are drawn with respect to the frames.
Therefore, DGM is calculated and applied to frames."""
transforms, cost = self.kinematics.get_joint_transforms(self.solve_loops)
self.check_cost(cost)
for i in range(len(self.frames)):
T = transforms[i]
f = self.frames[i]
f.pos = T[:3,3]
f.axis = T[:3,2]
f.up = T[:3,0]
# For prismatic joints the link length must be recomputed. Therefore, assigning those links to frames wouldn't be sufficient.
for cyl, jnt in self.prismatic_links:
cyl.axis = (-(jnt.r+jnt.q),0,0)
self.update_spin_controls()
######### EVENT HANDLERS ##########
def end_effector_change(self, evt):
self.show_end_effectors(evt.IsChecked())
def change_opacity(self, evt):
"""Slider event handler. Changes opacity of all joints
"""
self.opacity = evt.EventObject.GetValue()/100.
for obj in self.objs:
obj.opacity = self.opacity
def world_frame(self, evt):
"""Adds or removes the frame with labels for the base frame.
"""
if evt.EventObject.Value:
self.world_frame = frame(axis=(0,0,1), pos=(0,0,0), up=(1,0,0))
arrow(frame=self.world_frame, axis=(self.len_obj,0,0), color=color.red, shaftwidth=self.len_obj/25.)
label(frame=self.world_frame, text="z0", pos=(self.len_obj,0), opacity=0, yoffset=3, color=color.black, border=0, box=False, line=False)
arrow(frame=self.world_frame, axis=(0,self.len_obj,0), color=color.green, shaftwidth=self.len_obj/25.)
label(frame=self.world_frame, text="x0", pos=(0,self.len_obj), opacity=0, yoffset=3, color=color.black, border=0, box=False, line=False)
else:
self.world_frame.visible = False
del self.world_frame
def show_all_frames(self, evt):
"""Shows or hides all the frames (Toggle button event handler
"""
indices = range(len(self.frames)) if self.tButton.Value else []
self.show_frames(indices)
self.check_list.SetChecked(indices)
def update_spin_controls(self):
from random import random
print random()
for i, jnt in enumerate(self.kinematics.mjoints):
self.spin_ctrls[i].Value = jnt.q*180/pi if jnt.isrevolute() else jnt.q
def select_loops(self, evt):
"""Solve loops checkbox event handler. If checked, solves the loops immediately when variables are changed.
In this case passive variables are disabled for modification and calculated in terms of active variables.
"""
selection = self.radioBox.GetSelection()
if selection == 0:
self.solve_loops = True
self.redraw()
for i, ctrl in enumerate(self.spin_ctrls):
if self.kinematics.mjoints[i].ispassive():
ctrl.Enable(False)
self.btnReset.SetLabelText("Reset Active")
else:
self.solve_loops = False
for i, jnt in enumerate(self.kinematics.mjoints):
self.spin_ctrls[i].Enable(True)
self.update_spin_controls()
self.btnReset.SetLabelText("Reset All")
def find_close(self, evt):
"""Finds a solution from current configuration by changing all the joint variables (including active)
"""
if self.check_cost(self.kinematics.find_close()):
self.redraw()
def find_random(self, evt):
"""Finds a solution randomly initializing joint variables (including active)
"""
if self.contains_loops:
self.check_cost(self.kinematics.find_random(self.cost_tolerance))
self.radioBox.SetSelection(0)
self.select_loops(None)
else:
self.kinematics.random_qm()
self.redraw()
def reset_joints(self, evt):
"""Sets all the joint variables back. Useful for a user in case of loops
"""
for jnt in self.kinematics.mjoints:
jnt.q = 0
self.redraw()
def check_frames(self, evt):
"""Check list event handler. Shows frames which are selected
"""
self.show_frames(self.check_list.GetChecked())
self.tButton.Value = False
self.check_list.DeselectAll()
def setq(self, evt):
"""Sets joint values from the spin-controls
"""
from math import pi
jntIndex = evt.GetId()
jnt = self.kinematics.mjoints[jntIndex]
if jnt.isrevolute():
jnt.q = pi*evt.Value/180.
else:
jnt.q = evt.Value
self.redraw()
def changeRepresentation(self, evt):
"""Switch between to representations:
Normal view - all joints are located on their frames.
Expanded view - joints are shifted along z if their frames coincide.
"""
for obj in self.scene.objects:
if obj != self.world_frame and obj.frame != self.world_frame:
obj.visible = False
del obj
self.init_lists()
self.draw(evt.IsChecked())
def select(self, evt):
"""If a joint object is selected, highlights an object and shows only its frame
"""
self.show_frames(self.check_list.GetChecked())
for joint in self.objs:
joint.color = color.yellow if isinstance(joint, cylinder) else color.orange if isinstance(joint, box) else color.green
obj = evt.pick
if obj in self.objs:
obj.color = (obj.color[0]-0.3, obj.color[1]-0.3, obj.color[2]-0.3)
f = obj.frame
self.ch_func(self.objs.index(obj) + 1)
for o in self.frames_arrows:
if o.frame != f:
o.visible = False
else:
o.visible = True
# The next 3 functions are used for panning - Scene center is shifted to the mouse displacement distance (In the plane parallel to the display passing through scene.center)
def grab(self, evt):
self.drag_pos = evt.pos
self.scene.bind('mouseup', self.drop)
self.scene.bind('mousemove', self.move)
def move(self, evt):
self.scene.center += self.drag_pos - evt.pos
def drop(self, evt):
self.scene.unbind('mousemove', self.move)
self.scene.unbind('mouseup', self.drop)
import numpy as np
import utils
import constants as c
from kinematics import Kinematics, getInitialPlatformHeight
trans = [0, 0, 0]
rot = [-5, 5, 0]
base_anchor = utils.getAllBaseAnchor()
platform_anchor = utils.getAllPlatformAnchor()
beta_k = c.BETA_K
kin = Kinematics()
alpha = []
for i in range(6):
alpha.append(
kin.inverse_kinematics(trans, rot, base_anchor[i], platform_anchor[i],
beta_k[i]))
# print(base_anchor)
# print(platform_anchor)
# print(beta_k)
print(', '.join(str(x) for x in np.array(alpha) - np.array(c.REST_ANGLE)))
'''
+5 deg about x - SOUTH
[16.28929038, 17.3132488, 1.57780453, -1.42523919, -16.94926673, -16.62537064]
-5 deg about x - NORTH
[-16.33266239, -17.09930114, -1.54961138, 1.47727662, 17.42187392, 16.48079246]
+5 deg about y - EAST
def transform_joints(joints):
kdl = Kinematics()
kdl.set_kinematics('base_link', 'tcp0')
poses = [kdl.get_pose(jnts) for jnts in joints]
return poses
# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
# THE SOFTWARE.
#
# pylint: disable=C0103
"""Simple test for servo actuation"""
import math
from random import randint
from kinematics import Kinematics
random = True
kinematics = Kinematics(False)
def random_angle() -> float:
"""Returns a random angle betwee 89.5 and -89.5 degress
:return: Random angle between 89.5 and -89.5 degrees.
:rtype: float
"""
a = randint(-89.5, 89.5) * 1.0
return a
def random_test():
"""Performs kinematics and reerse kinematics test on random set of points.
"""
count = 0
class KinematicsTest(unittest.TestCase):
"""
Tests inverse kinematics
"""
def setUp(self):
print("<Setup START>")
links = [26, 22, 12.5]
self.kinematics = Kinematics(links, 0, 0, 0, 0)
def test_inverse_kuka_initial(self):
"""
Tests the following inverse kinematics problem:
1) "KUKA Initial" Joint 3 fixed:
0 on base, 1.51 on joint 1, 1.48 on joint 2, 0 on joint 3, 0 on joint 4:
26, 0, 36
"""
print("<Test Inverse KUKA initial START>")
x, y, z = 36, 0, 26
fixed_joint = 3
angle = 0
results = self.kinematics.inverse(x, y, z, fixed_joint, angle)
expected = [0.0, 1.51, -1.51, 0.0]
result = all((abs(x - y) < 0.01 for x, y in zip(results, expected)))
self.assertTrue(result)
def test_inverse_touch_ground(self):
"""
Tests the following inverse kinematics problem:
1) "touch ground" Axis 3 fixed:
0 on base, 0.60 on joint 1, 0.80 on joint 2, 0.79 on joint 3, 0 on joint 4:
50, 0, 0
"""
print("<Test touch ground initial START>")
x, y, z = 50, 0, 0
fixed_joint = 3
angle = -0.79
results = self.kinematics.inverse(x, y, z, fixed_joint, angle)
print(results)
expected = [0.0, 0.60, -0.80, -0.79]
result = all((abs(x - y) < 0.01 for x, y in zip(results, expected)))
self.assertTrue(result)
def test_inverse_zick_zack(self):
"""
Tests the following inverse kinematics problem:
1) "Zick Zack" Axis 3 fixed:
0 on base, 1.36 on joint 1, 2.46 on joint 2, -2.45 on joint 3, 0 on joint 4:
18, 0, 18
"""
print("<Test Zick Zack START>")
x, y, z = 18, 0, 18
fixed_joint = 3
angle = 2.45
results = self.kinematics.inverse(x, y, z, fixed_joint, angle)
print(results)
expected = [0.0, 1.36, -2.46, 2.45]
result = all((abs(x - y) < 0.01 for x, y in zip(results, expected)))
self.assertTrue(result)
def test_inverse_rear_low(self):
"""
Tests the following inverse kinematics problem:
1) "rear low" Axis 3 fixed:
0 on base, 1.73 on joint 1, 2.50 on joint 2, -0.75 on joint 3, 0 on joint 4:
24, 0, 10
"""
print("<Test rear low START>")
x, y, z = 24, 0, 10
fixed_joint = 3
angle = 0.75
results = self.kinematics.inverse(x, y, z, fixed_joint, angle)
print(results)
expected = [0.0, 1.73, -2.5, 0.75]
result = all((abs(x - y) < 0.01 for x, y in zip(results, expected)))
self.assertTrue(result)
def test_inverse_rear_high(self):
"""
Tests the following inverse kinematics problem:
1) "rear high" Axis 3 fixed:
0 on base, 1.78 on joint 1, 1.08 on joint 2, 0.65 on joint 3, 0 on joint 4:
24, 0, 40
"""
print("<Test rear high START>")
x, y, z = 24, 0, 40
fixed_joint = 3
angle = -0.65
results = self.kinematics.inverse(x, y, z, fixed_joint, angle)
print(results)
expected = [0.0, 1.78, -1.08, -0.65]
result = all((abs(x - y) < 0.01 for x, y in zip(results, expected)))
self.assertTrue(result)
def test_inverse_singularity(self):
"""
Tests the following inverse kinematics problem:
1) "singularity" Axis 3 fixed:
0 on base, 1.85 on joint 1, 1.06 on joint 2, -1.5 on joint 3, 0 on joint 4:
50, 0, 0
"""
print("<Test touch singularity START>")
x, y, z = 0, 0, 50
fixed_joint = 3
angle = 1.5
results = self.kinematics.inverse(x, y, z, fixed_joint, angle)
print(results)
expected = [0.0, 1.85, -1.06, 1.5]
result = all((abs(x - y) < 0.01 for x, y in zip(results, expected)))
self.assertTrue(result)
def test_inverse_first_axis_fixed(self):
"""
5) Axis 1 fixed:
0° on base, 1.35 on axis 1, ~1.1 on axis 2, ~1.03 on axis 3:
35.9, 0, 22.02
"""
print("<Test Inverse First Axis Fixed START>")
x, y, z = 35.9, 0, 22.02
fixed_joint = 1
angle = 1.35
results = self.kinematics.inverse(x, y, z, fixed_joint, angle)
expected = [0.0, 1.35, -1.1, -1.03]
result = all((abs(x - y) < 0.01 for x, y in zip(results, expected)))
self.assertTrue(result)
def test_inverse_too_far(self):
"""
Tests the following inverse kinematics problem:
11) "Way too far":
not reachable
500, 500, 500
"""
print("<Test Inverse too far START>")
x, y, z = 500, 500, 500
fixed_joint = 3
self.assertRaises(CalculationError, self.kinematics.inverse, x, y, z, fixed_joint, 0)
def test_inverse_base_fixed(self):
"""
Tests the following inverse kinematics problem:
21) "Base is fixed":
The base cannot be the fixed one
"""
print("<Test inverse base fixed START>")
x, y, z = 26, 0, 36
fixed_joint = 0
self.assertRaises(ParameterError, self.kinematics.inverse, x, y, z, fixed_joint, 0)