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 __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 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 __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 __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 __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 __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 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 __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 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
def __init__(self,
controller: PCA9685,
hip_channel: int = 15,
elbow_channel: int = 12,
shoulder_channel: int = 13,
gripper_channel: int = 14,
initialize: bool = True,
logging_level: str = 'INFO'):
"""__init__
Default initialization of arm. Avoid using this and instead create a meArm using the meArm.createWithParameters
method, which ensures that a meArm is not registered twice.
:param controller: The controller to which the arm is attached.
:type controller: PCA9685
:param hip_channel: The channel for the hip servo.
:type hip_channel: int
:param elbow_channel: The channel for the elbow servo.
:type elbow_channel: int
:param shoulder_channel: The channel for the shoulder servo.
:type shoulder_channel: int
:param gripper_channel: The channel for the gripper servo.
:type gripper_channel: int
:param initialize: True to immidiately run the servo initialization, false to adjuist values after construction.
:type initialize: bool
:param logging_level: The logging level to use for this arm.
:type logging_level: string
"""
self._servo_tag: str = str(hip_channel).zfill(2) + str(
elbow_channel).zfill(2) + str(shoulder_channel).zfill(2) + str(
gripper_channel).zfill(2)
self._id: str = str(controller.address).zfill(6) + self._servo_tag
self._logger = logging.getLogger("%s.%s" % (__name__, self._id))
self._logger.setLevel(logging_level)
if hip_channel < 0 or hip_channel > 15 or \
elbow_channel < 0 or elbow_channel > 15 or \
shoulder_channel < 0 or shoulder_channel > 15 or \
gripper_channel < 0 or gripper_channel > 15:
msg = "Servo channel values must be between 0 and 15"
self._logger.error(msg)
raise ValueError(msg)
if controller is None:
msg = "You must supply a valid controller object to create a meArm"
self._logger.error(msg)
raise Exception(msg)
if self._id in me_arm._instances:
msg = "meArm Instance already exists. Cannot create a new instance. Release the existing \
instance by calling me_arm.delete()"
self._logger.error(msg)
raise Exception(msg)
self._controller = controller
self._kinematics = Kinematics()
self._turnedOff = False
self.__setup_defaults(hip_channel, elbow_channel, shoulder_channel,
gripper_channel)
if initialize: self.initialize()
me_arm._instances[self._id] = self
if controller.address not in me_arm._controllers:
me_arm._controllers.append(controller.address)
self._logger.info("meArm with id %s created", self._id)
'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 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)
# 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
def main():
controller_knee = Controller(controller_ID = 1)
controller_hip = Controller(controller_ID = 2)
controller_abad = Controller(controller_ID=3)
response_data_c1 = controller_knee.get_data()
robot_knee_rot = response_data_c1[MoteusReg.MOTEUS_REG_POSITION]
response_data_c2 = controller_hip.get_data()
robot_hip_rot = response_data_c2[MoteusReg.MOTEUS_REG_POSITION]
kinematics = Kinematics(robot_knee_rot, robot_hip_rot)
freq=300
period =1
jump_duration = 0.1
idle_duration = 0.2 # 0.35
jump_torque = 2
land_torque = 1.4
jump_stance_low = 70
jump_stance_high = 280
x_pos = 0
retract_duration = period - jump_duration - idle_duration
begin_time=time.time()
while True:
freq_measure_time = time.time()
phase = (time.time()-begin_time+idle_duration+jump_duration) % (period)
if phase <= jump_duration:
jump_phase = phase/jump_duration
x = -x_pos
z = jump_stance_low + (jump_stance_high-jump_stance_low)*jump_phase
if kinematics.if_ik_possible(x, z):
robot_hip_rot, robot_knee_rot = kinematics.ik(x, z)
controller_hip.set_position(position=robot_hip_rot, max_torque=jump_torque, kd_scale=0.4, kp_scale=1)
controller_knee.set_position(position=robot_knee_rot, max_torque=jump_torque, kd_scale=0.4, kp_scale=1)
if (period - retract_duration) > phase >= jump_duration:
idle_phase = (phase - jump_duration) / idle_duration # normalize to 0-1
x = -x_pos-0*idle_phase
z = jump_stance_high
# z = jump_stance_high - 10 - 40 * idle_phase
# if idle_phase<0.25:
# x=-60*(idle_phase*4) -15
# elif idle_phase<0.5:
# x=60-120*(idle_phase*2-0.5) -15
# else:
# x=-60+60* (idle_phase * 4 - 3) -15
if kinematics.if_ik_possible(x, z):
robot_hip_rot, robot_knee_rot = kinematics.ik(x, z)
controller_hip.set_position(position=robot_hip_rot, max_torque=jump_torque, kd_scale=0.2, kp_scale=1)
controller_knee.set_position(position=robot_knee_rot, max_torque=jump_torque, kd_scale=0.2, kp_scale=1)
else:
print(x,z)
if phase > (period-retract_duration):
retract_phase = (phase - jump_duration-idle_duration) / retract_duration # normalize to 0-1
x = -x_pos + 0*(1-retract_phase)
z = jump_stance_high - (jump_stance_high-jump_stance_low) * retract_phase
if kinematics.if_ik_possible(x, z):
robot_hip_rot, robot_knee_rot = kinematics.ik(x, z)
controller_hip.set_position(position=robot_hip_rot, max_torque=land_torque, kd_scale=1, kp_scale=0.3)
controller_knee.set_position(position=robot_knee_rot, max_torque=land_torque, kd_scale=1, kp_scale=0.3)
# if kinematics.if_ik_possible(x, z):
# robot_hip_rot_calculated, robot_knee_rot_calculated = kinematics.ik(x, z)
#
# response_data_c1 = controller_knee.get_data()
# robot_knee_rot_measured = response_data_c1[MoteusReg.MOTEUS_REG_POSITION]
# response_data_c2 = controller_hip.get_data()
# robot_hip_rot_measured = response_data_c2[MoteusReg.MOTEUS_REG_POSITION]
#
# #print(robot_hip_rot_calculated, robot_hip_rot_measured, robot_knee_rot_calculated, robot_hip_rot_measured)
#
# controller_hip.set_position(position=robot_hip_rot_calculated, max_torque=1.5, kd_scale=0.8, kp_scale=1.2)
# controller_knee.set_position(position=robot_knee_rot_calculated, max_torque=1.5, kd_scale=0.8, kp_scale=1.2)
sleep = (1/freq)-(time.time()-freq_measure_time)
if sleep < 0: sleep = 0
time.sleep(sleep)
#!/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])])
def transform_joints(joints):
kdl = Kinematics()
kdl.set_kinematics('base_link', 'tcp0')
poses = [kdl.get_pose(jnts) for jnts in joints]
return poses
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()
kinematicsType = 'Quadrilateral'
isQuadKinematics = BooleanProperty(True)
def initialize(self):
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.chainSagCorrectionOffset.bind(value=self.onSliderChange)
self.vertCGDist.bind(value=self.onSliderChange)
self.gridSize.bind(value=self.onSliderChange)
Clock.schedule_once(self.moveToCenter, 3)
#start with our current kinematics type
self.kinematicsSelect.text = self.data.config.get(
'Advanced Settings', 'kinematicsType')
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):
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.chainSagCorrectionOffset.value = 0
self.gridSize.value = 300
def recompute(self):
#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()
self.scatterInstance.canvas.clear()
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):
lengthMM = 800
#horizontal measurement
pointPlotted1, distortedPoint1 = self.testPointGenerator.plotPoint(
-lengthMM / 2, 0)
pointPlotted2, distortedPoint2 = self.testPointGenerator.plotPoint(
lengthMM / 2, 0)
#vertical measurement
pointPlotted3, distortedPoint3 = self.testPointGenerator.plotPoint(
0, lengthMM / 2)
pointPlotted4, distortedPoint4 = self.testPointGenerator.plotPoint(
0, -lengthMM / 2)
printString = "An 800mm square centered on the sheet is distorted\n%.4fmm horizontally, and %.4fmm vertically." % (
(lengthMM - (distortedPoint2[0] - distortedPoint1[0])),
(lengthMM - (distortedPoint3[1] - distortedPoint4[1])))
lengthMM = 1905 / 2
pointPlotted1, distortedPoint1 = self.testPointGenerator.plotPoint(
-lengthMM / 2, -400)
pointPlotted2, distortedPoint2 = self.testPointGenerator.plotPoint(
lengthMM / 2, -400)
printString1 = printString + "\n\nThe triangular calibration test is off by %.4fmm." % (
(lengthMM - (distortedPoint2[0] - distortedPoint1[0])))
lengthMM = 800
xOffset = 1100 - lengthMM / 2
yOffset = 500 - lengthMM / 2
#horizontal measurement
pointPlotted1, distortedPoint1 = self.testPointGenerator.plotPoint(
-lengthMM / 2, yOffset)
pointPlotted2, distortedPoint2 = self.testPointGenerator.plotPoint(
lengthMM / 2, yOffset)
#vertical measurement
pointPlotted3, distortedPoint3 = self.testPointGenerator.plotPoint(
xOffset, lengthMM / 2)
pointPlotted4, distortedPoint4 = self.testPointGenerator.plotPoint(
xOffset, -lengthMM / 2)
printString = "An 800mm square in the top corner on the sheet is distorted\n%.4fmm horizontally, and %.4fmm vertically." % (
(lengthMM - (distortedPoint2[0] - distortedPoint1[0])),
(lengthMM - (distortedPoint3[1] - distortedPoint4[1])))
lengthMM = 800
xOffset = 1100 - lengthMM / 2
yOffset = 500 - lengthMM / 2
#horizontal measurement
pointPlotted1, distortedPoint1 = self.testPointGenerator.plotPoint(
-lengthMM / 2, -yOffset)
pointPlotted2, distortedPoint2 = self.testPointGenerator.plotPoint(
lengthMM / 2, -yOffset)
#vertical measurement
pointPlotted3, distortedPoint3 = self.testPointGenerator.plotPoint(
xOffset, lengthMM / 2)
pointPlotted4, distortedPoint4 = self.testPointGenerator.plotPoint(
xOffset, -lengthMM / 2)
printString2 = printString + "\n\nAn 800mm square in the bottom corner on the sheet is distorted\n%.4fmm horizontally, and %.4fmm vertically." % (
(lengthMM - (distortedPoint2[0] - distortedPoint1[0])),
(lengthMM - (distortedPoint3[1] - distortedPoint4[1])))
#calculate "Calibration Benchmark Score"
#dimensions used in calibration cuts
scoreXMM = 1905.0
scoreYMM = 900.0
scoreBoxMM = 100.0
#plot horizontal measurement
pointPlotted1h, distortedPoint1h = self.testPointGenerator.plotPoint(
-scoreXMM / 2.0, scoreYMM / 2.0)
pointPlotted2h, distortedPoint2h = self.testPointGenerator.plotPoint(
scoreXMM / 2.0, scoreYMM / 2.0)
errHTop = abs(scoreXMM -
abs(distortedPoint1h[0] - distortedPoint2h[0]))
pointPlotted3h, distortedPoint3h = self.testPointGenerator.plotPoint(
-scoreXMM / 2.0, 0.0)
pointPlotted4h, distortedPoint4h = self.testPointGenerator.plotPoint(
scoreXMM / 2.0, 0.0)
errHMid = abs(scoreXMM -
abs(distortedPoint3h[0] - distortedPoint4h[0]))
pointPlotted5h, distortedPoint5h = self.testPointGenerator.plotPoint(
-scoreXMM / 2.0, -scoreYMM / 2.0)
pointPlotted6h, distortedPoint6h = self.testPointGenerator.plotPoint(
scoreXMM / 2.0, -scoreYMM / 2.0)
errHBtm = abs(scoreXMM -
abs(distortedPoint5h[0] - distortedPoint6h[0]))
# print "errHTop " + str(errHTop)
# print "errHMid " + str(errHMid)
# print "errHBtm " + str(errHBtm)
#plot vertical measurements
pointPlotted1v, distortedPoint1v = self.testPointGenerator.plotPoint(
-scoreXMM / 2.0, scoreYMM / 2.0)
pointPlotted2v, distortedPoint2v = self.testPointGenerator.plotPoint(
-scoreXMM / 2.0, -scoreYMM / 2.0)
errVLft = abs(scoreYMM -
abs(distortedPoint1v[1] - distortedPoint2v[1]))
pointPlotted3v, distortedPoint3v = self.testPointGenerator.plotPoint(
0.0, scoreYMM / 2.0)
pointPlotted4v, distortedPoint4v = self.testPointGenerator.plotPoint(
0.0, -scoreYMM / 2.0)
errVMid = abs(scoreYMM -
abs(distortedPoint3v[1] - distortedPoint4v[1]))
pointPlotted5v, distortedPoint5v = self.testPointGenerator.plotPoint(
scoreXMM / 2.0, scoreYMM / 2.0)
pointPlotted6v, distortedPoint6v = self.testPointGenerator.plotPoint(
scoreXMM / 2.0, -scoreYMM / 2.0)
errVRht = abs(scoreYMM -
abs(distortedPoint5v[1] - distortedPoint6v[1]))
# print "errVLft " + str(errVLft)
# print "errVMid " + str(errVMid)
# print "errVRht " + str(errVRht)
#combine the two averages for the first part of the cmScore
cmScoreBig = (errHTop + errHMid + errHBtm + errVLft + errVMid +
errVRht) / 6.0
#plot box measurements
pointPlotted0b, distortedPoint0b = self.testPointGenerator.plotPoint(
-scoreXMM / 2.0, scoreYMM / 2.0)
pointPlotted1b, distortedPoint1b = self.testPointGenerator.plotPoint(
-scoreXMM / 2.0 + scoreBoxMM, scoreYMM / 2.0 - scoreBoxMM)
pointPlotted2b, distortedPoint2b = self.testPointGenerator.plotPoint(
-scoreXMM / 2.0, -scoreYMM / 2.0)
pointPlotted3b, distortedPoint3b = self.testPointGenerator.plotPoint(
-scoreXMM / 2.0 + scoreBoxMM, -scoreYMM / 2.0 + scoreBoxMM)
pointPlotted4b, distortedPoint4b = self.testPointGenerator.plotPoint(
-scoreBoxMM / 2.0, scoreBoxMM / 2.0)
pointPlotted5b, distortedPoint5b = self.testPointGenerator.plotPoint(
scoreBoxMM / 2.0, -scoreBoxMM / 2.0)
pointPlotted6b, distortedPoint6b = self.testPointGenerator.plotPoint(
scoreXMM / 2.0, scoreYMM / 2.0)
pointPlotted7b, distortedPoint7b = self.testPointGenerator.plotPoint(
scoreXMM / 2.0 - scoreBoxMM, scoreYMM / 2.0 - scoreBoxMM)
pointPlotted8b, distortedPoint8b = self.testPointGenerator.plotPoint(
scoreXMM / 2.0, -scoreYMM / 2.0)
pointPlotted9b, distortedPoint9b = self.testPointGenerator.plotPoint(
scoreXMM / 2.0 - scoreBoxMM, -scoreYMM / 2.0 + scoreBoxMM)
#calculate second half of "Calibration Benchmark Score" here, X-directon
scoreBoxX1 = abs(scoreBoxMM -
abs(distortedPoint0b[0] - distortedPoint1b[0]))
scoreBoxX2 = abs(scoreBoxMM -
abs(distortedPoint2b[0] - distortedPoint3b[0]))
scoreBoxX3 = abs(scoreBoxMM -
abs(distortedPoint4b[0] - distortedPoint5b[0]))
scoreBoxX4 = abs(scoreBoxMM -
abs(distortedPoint6b[0] - distortedPoint7b[0]))
scoreBoxX5 = abs(scoreBoxMM -
abs(distortedPoint8b[0] - distortedPoint9b[0]))
scoreBoxX = (scoreBoxX1 + scoreBoxX2 + scoreBoxX3 + scoreBoxX4 +
scoreBoxX5) / 5.0
#calculate second half of "Calibration Benchmark Score" here, Y-directon
scoreBoxY1 = abs(scoreBoxMM -
abs(distortedPoint0b[1] - distortedPoint1b[1]))
scoreBoxY2 = abs(scoreBoxMM -
abs(distortedPoint2b[1] - distortedPoint3b[1]))
scoreBoxY3 = abs(scoreBoxMM -
abs(distortedPoint4b[1] - distortedPoint5b[1]))
scoreBoxY4 = abs(scoreBoxMM -
abs(distortedPoint6b[1] - distortedPoint7b[1]))
scoreBoxY5 = abs(scoreBoxMM -
abs(distortedPoint8b[1] - distortedPoint9b[1]))
scoreBoxY = (scoreBoxY1 + scoreBoxY2 + scoreBoxY3 + scoreBoxY4 +
scoreBoxY5) / 5.0
# print str(scoreBoxX1) + " " + str(scoreBoxY1)
# print str(scoreBoxX2) + " " + str(scoreBoxY2)
# print str(scoreBoxX3) + " " + str(scoreBoxY3)
# print str(scoreBoxX4) + " " + str(scoreBoxY4)
# print str(scoreBoxX5) + " " + str(scoreBoxY5)
#combine the two averages for the second part of the cmScore
cmScoreBox = (scoreBoxX + scoreBoxY) / 2.0
printString1 = printString1 + "\r\n\r\nThe Calibration Benchmark Score is %.3f - %.3f" % (
cmScoreBig, cmScoreBox)
self.machineLabel1.text = printString1
self.machineLabel2.text = printString2
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: " + "%.2f" % self.motorVerticalError.value + "mm"
self.distortedKinematics.l = self.correctKinematics.l + self.sledMountSpacingError.value
self.sledMountSpacingErrorLabel.text = "Sled Mount\nSpacing Error: " + "%.2f" % self.sledMountSpacingError.value + "mm"
self.distortedKinematics.D = self.correctKinematics.D + self.motorSpacingError.value
self.motorSpacingErrorLabel.text = "Motor Spacing\nError: " + "%.2f" % self.motorSpacingError.value + "mm"
self.distortedKinematics.s = self.correctKinematics.s + self.vertBitDist.value
self.vertBitDistLabel.text = "Vert Dist To\nBit Error: " + "%.2f" % self.vertBitDist.value + "mm"
self.distortedKinematics.h3 = self.correctKinematics.h3 + self.vertCGDist.value
self.vertCGDistLabel.text = "Vert Dist\nBit to CG Error: " + "%.2f" % self.vertCGDist.value + "mm"
self.distortedKinematics.chain1Offset = int(self.leftChainOffset.value)
self.leftChainOffsetLabel.text = "Left Chain\nSlipped Links: " + "%.2f" % self.leftChainOffset.value + "links"
self.distortedKinematics.chain2Offset = int(
self.rightChainOffset.value)
self.rightChainOffsetLabel.text = "Right Chain\nSlipped Links: " + "%.2f" % self.rightChainOffset.value + "links"
self.distortedKinematics.rotationDiskRadius = self.correctKinematics.rotationDiskRadius + self.rotationRadiusOffset.value
self.rotationRadiusLabel.text = "Rotation Radius\nSpacing Error: " + "%.2f" % self.rotationRadiusOffset.value + "mm"
self.distortedKinematics.chainSagCorrection = self.correctKinematics.chainSagCorrection + self.chainSagCorrectionOffset.value
self.chainSagCorrectionLabel.text = "Chain Sag\Correction Error: " + "%.2f" % self.chainSagCorrectionOffset.value
#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)
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()
def constructKinematics(calibration):
''' construct kinematics from calibration object '''
return Kinematics(calibration)
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
#!/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()
c1 = cos(q0)
# 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 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()
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)
#scale it down to fit on the screen
self.scatterInstance.apply_transform(Matrix().scale(.3, .3, 1))
mat = Matrix().translate(500, 500, 0)
self.scatterInstance.apply_transform(mat)
self.recompute()
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
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
horizontalStepSize = (2 * leftRigthBound) / 12
verticalStepSize = (2 * topBottomBound) / 7
self.verticalPoints = range(topBottomBound, -topBottomBound, -200)
self.horizontalPoints = range(-leftRigthBound, leftRigthBound,
horizontalStepSize)
#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):
#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])
points.append(point[1])
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])
points.append(point[1])
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])
points.append(point[1])
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])
points.append(point[1])
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: " + str(lengthMM -
(distortedPoint2[0] - distortedPoint1[0]))
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.recomputeGeometry()
def drawOutline(self):
bedWidth = self.correctKinematics.machineWidth
bedHeight = self.correctKinematics.machineHeight
with self.scatterInstance.canvas:
Line(points=(-bedWidth / 2, -bedHeight / 2, -bedWidth / 2,
bedHeight / 2))
Line(points=(bedWidth / 2, -bedHeight / 2, bedWidth / 2,
bedHeight / 2))
Line(points=(-bedWidth / 2, -bedHeight / 2, +bedWidth / 2,
-bedHeight / 2))
Line(points=(-bedWidth / 2, bedHeight / 2, +bedWidth / 2,
bedHeight / 2))
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 compute_joint_state(self, cv_image1, cv_image2):
self.joint_state.cv_image1 = cv_image1
self.joint_state.cv_image2 = cv_image2
kinematics = Kinematics()
result = self.joint_state.compute_joint_state(False)
if (result is not None):
target_position = self.joint_state.compute_target_position(False)
if (target_position is not None):
target_position_meters = target_position * self.joint_state.meter_to_pixel_factor
# Publish target position
self.target_pub.publish(
Float64MultiArray(data=target_position_meters))
k = kinematics.kinematics(self.joint_state)
# print("K")
# print(k[0:3,-1:])
# print("red")
# print(self.joint_state.red_pos * self.joint_state.meter_to_pixel_factor)
# Publish Kinematics
self.kinematics_pub.publish(Float64MultiArray(data=k[0:3,
-1:]))
# Publish red joint position
self.red_pub.publish(
Float64MultiArray(data=self.joint_state.red_pos *
self.joint_state.meter_to_pixel_factor))
new_joint_state = self.joint_state.control_closed(True)
if (new_joint_state is not None):
self.robot_joint1_pub.publish(new_joint_state[0])
self.robot_joint2_pub.publish(new_joint_state[1])
self.robot_joint3_pub.publish(new_joint_state[2])
self.robot_joint4_pub.publish(new_joint_state[3])
# # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # #
# cv2.circle(cv_image1, (int(k[1, 3] / self.joint_state.meter_to_pixel_factor + self.joint_state.origin_pos[1]),
# int(-k[2, 3] / self.joint_state.meter_to_pixel_factor + self.joint_state.origin_pos[2])),
# 10, (0, 0, 255), thickness=4, lineType=8)
#
# cv2.circle(cv_image1, (int(joint_state.origin_pos[1]),
# int(joint_state.origin_pos[2])),
# 18, (0, 255, 255), thickness=4, lineType=8)
# cv2.circle(cv_image1, (int(joint_state.blue_abs_pos[1]),
# int(joint_state.blue_abs_pos[2])),
# 16, (255, 0, 0), thickness=4, lineType=8)
# cv2.imshow('k1', cv_image1)
# cv2.waitKey(1)
#
# cv2.circle(cv_image2, (int(k[0, 3] / self.joint_state.meter_to_pixel_factor + self.joint_state.origin_pos[0]),
# int(-k[2, 3] / self.joint_state.meter_to_pixel_factor + self.joint_state.origin_pos[2])),
# 10, (0, 0, 255), thickness=4, lineType=8)
# cv2.circle(cv_image1,
# (int(joint_positions1['green'][0]), int(joint_positions1['green'][1])), 10,
# (255, 0, 255), thickness=2, lineType=8)
# cv2.circle(cv_image1,
# (int(joint_positions1['blue'][0]), int(joint_positions1['blue'][1])), 10,
# (255, 0, 255), thickness=2, lineType=8)
# cv2.circle(cv_image2,
# #(int(joint_state.red_abs_pos[0]), int(joint_state.red_abs_pos[2])), 5,
# (int(3 / self.joint_state.meter_to_pixel_factor + self.joint_state.origin_pos[0]),
# int(-5 / self.joint_state.meter_to_pixel_factor + self.joint_state.origin_pos[2])), 10,
# (0, 10, 205), thickness=2, lineType=8)
# cv2.imshow('k2', cv_image2)
# cv2.waitKey(1)
else:
print("Skipping")
else:
print("Skipping")
dh8 = DHDef(8, 'R', 0, -sympy.pi / 2, 0, q8 + sympy.pi, 'mdh', dh1, []) dh9 = DHDef(9, 'R', 0.1, 0, 0, q9 - sympy.pi / 2, 'mdh', dh8, []) dh0.set_succ([dh1]) dh1.set_succ([dh2, dh8]) dh2.set_succ([dh3]) dh3.set_succ([dh4]) dh4.set_succ([dh5]) dh5.set_succ([dh6]) dh6.set_succ([dh7]) dh8.set_succ([dh9]) start_time = time.time() kin = Kinematics(dh0) kin.cal_transfmats() print(kin._coordinates) print(kin._coordinates_t) print(kin._d_coordinates) print(kin._d_coordinates_t) print(kin._dd_coordinates) print(kin._dd_coordinates_t) #kin.draw_frames() dyn = Dynamics(kin) print(dyn._ml2r(dh1._m, dh1._l)) print(dyn._Lmr2I(dh1._L_mat, dh1._m, dh1._r)) dyn.cal_dynamics() print(dyn._tau)