We came together with a vague understanding of the movement we wanted to compute. Upon further diving into the spiral example code, we were intrigued with the possibilities presented by the marker. This brought up an interesting challenge where we could incorporate another component to this “duet”.
Our goal then became to integrate the marker with the motion of the two arms.
After trying to output the locations of the last dot, we could find a way to get the two arms to connect via the dots. In our search, we noted that the markers were beginning to emulate “ribbons.” From there, we moved on and began trying to create a synchronized ribbon dancing pair of arms.
After successfully syncing our ribbon dancers, we thought it looked too robotic and not quite plausible. In response, we staggered the two arms to make it a bit more clear that the right arm was following the left arm. This gives the arms a bit more personality and character that comes across in the dance.
Code: We updated the pendulumcontroller to have one mimic the state of the other using the PD feedback. It also places a new marker for every frame for each arm, to simulate a ribbon.
#!/usr/bin/env python3
"""Prototype Python 3 script for the 16-375 double-pendulum control exercise."""
################################################################
# Written in 2018-2019 by Garth Zeglin <garthz@cmu.edu>
# To the extent possible under law, the author has dedicated all copyright
# and related and neighboring rights to this software to the public domain
# worldwide. This software is distributed without any warranty.
# You should have received a copy of the CC0 Public Domain Dedication along with this software.
# If not, see <http://creativecommons.org/publicdomain/zero/1.0/>.
################################################################
import rcp.doublependulum
from rcp.ex.ndblpend import main
import numpy as np
from PyQt5.QtGui import QPainter, QColor, QPen
################################################################
class PendulumController(rcp.doublependulum.DoublePendulumController):
def __init__(self):
super().__init__()
# override the default initial state
self.initial_state = np.array([3.0, 0.0, 0.0, 0.0])
return
#================================================================
# Specialize this particular controller when a member of an ensemble.
def set_identity(self, serial_number):
super().set_identity(serial_number)
# perturb the initial state
self.initial_state[0] += self.identity * 0.05
return
#================================================================
def setup(self):
if self.identity == 0:
self.write("""Passive double-pendulum simulation.
A zero torque vector is applied to the joints so they swing freely. The underlying simulation has frictionless joints so the behavior is chaotic. It is ultimately unstable due to accumulated numerical error.\n""")
return
#================================================================
def compute_control(self, t, dt, state, tau):
"""Method called from simulator to calculate the next step of applied torques.
:param t: time in seconds since simulation began
:param state: four element numpy array with joint positions ``q`` and joint velocities ``qd`` as ``[q0, q1, qd0, qd1]``, expressed in radians and radians/sec
:param tau: two element numpy array to fill in with the joint torques to apply ``[tau0, tau1]``
"""
tau[0:2] = (0,0)
elbow, end = self.model.forwardKinematics(state[0:2])
if self.identity == 1:
#target = np.concatenate((self.world.get_some_angles(), np.zeros(2)))
target = self.world.get_some_angles()
target[3] /= 1.02
target[2] /= 1.01
# calculate position and velocity error as difference from reference state
qerr = target - state
# apply PD control to reach the pose (no integral term)
# P constant decreases as the robot gets "tired"
tau[0] = ((self.kp[0]) * qerr[0]) + (self.kd[0] * qerr[2])
tau[1] = ((self.kp[1]) * qerr[1]) + (self.kd[1] * qerr[3])
else:
tau[0] = 0
tau[1] = 0
if self.identity == 0:
self.world.set_some_angles(state)
if self.identity == 0:
self.world.set_marker(int(((t*200)//1 % 100) * 2), (end[0] - 0, end[1]))
else:
self.world.set_marker(int(((t*200)//1 % 100) * 2 + 1), (end[0] - 0, end[1]))
return
################################################################$
if __name__ == "__main__":
main(PendulumController)
To allow the right arm to see the positions of the angles of the left arm, we had to add two functions to the MainApp class:
def set_some_angles(self, angles):
self.angles = angles
def get_some_angles(self):
return self.angles
To make the ribbons, we had to modify the marker to increment the hue each frame, and to allow up to 200 markers instead of just 1. We updated the setupUi function to increment the hue for each new marker:
self.markers = []
lastColor = QColor(255,0,0)
for i in range(200):
newcolor = copy.copy(lastColor)
print(lastColor, newcolor)
hsl = lastColor.getHslF()
hsl = ((hsl[0] + 0.005) % 1, hsl[1], hsl[2])
newcolor.setHslF(*hsl)
lastColor = newcolor
print(lastColor, newcolor)
marker = rcp.QtDoublePendulum.QtMarkerItem(color=newcolor)
marker.setPos(2000, 0)
self.markers.append(marker)
self.scene.addItem(marker)
We modified the markers so you can set the color instead of hardcoded green:
class QtMarkerItem(QtWidgets.QGraphicsItem):
def __init__(self, parent = None, color=QtCore.Qt.green):
super().__init__(parent)
self.bounds = QtCore.QRectF(-25, -25, 50, 50)
self.color = color
return
def boundingRect(self):
return self.bounds
def paint(self, painter, options, widget):
qp = painter
# set up green fill with black outlines
#pen = QtGui.QPen(QtCore.Qt.black)
#pen.setWidthF(3.0)
qp.setPen(QtCore.Qt.NoPen)
brush = QtGui.QBrush(self.color)
qp.setBrush(brush)
# draw the marker as a circle
qp.drawEllipse(QtCore.QPointF(0, 0), 20, 20)
return
def dim(self):
self.color.setAlphaF(self.color.alphaF() - 0.01 if self.color.alphaF() >= 0.01 else 0)
def undim(self):
self.color.setAlphaF(1)
Comments are closed.