Example #1
0
 def initJoints(self):
     self.shoulderYaw = Antagonist("shoulder_yaw")
     self.shoulderRoll = Antagonist("shoulder_roll")
     self.shoulderPitch = Antagonist("shoulder_pitch")
     self.upperarmRoll = DirectDrive("upperarm_roll", self.pi)
     self.elbow = Antagonist("elbow")
     self.forearmRoll = DirectDrive("forearm_roll", 1.75*self.pi)
     self.wrist = Antagonist("wrist")
     self.headYaw = DirectDrive("head_yaw", 1.5*self.pi)
Example #2
0
 def initJoints(self):
     self.shoulderYaw = Antagonist("shoulder_yaw")
     self.shoulderRoll = Antagonist("shoulder_roll")
     self.shoulderPitch = Antagonist("shoulder_pitch")
     self.upperarmRoll = DirectDrive("upperarm_roll", self.pi)
     self.elbow = Antagonist("elbow")
     self.forearmRoll = DirectDrive("forearm_roll", 1.75*self.pi)
     self.wrist = Antagonist("wrist")
     self.headYaw = DirectDrive("head_yaw", 1.5*self.pi)
Example #3
0
    def initJoints(self):
        for name in self.joints.keys():
            if self.joints[name]['antagonist']:
                self.joints[name]['controller'] = Antagonist(name)
            else:
                self.joints[name]['controller'] = DirectDrive(name)

            self.joints[name]['position'] = self.joints[name][
                'controller'].getJointAngle()
            self.joints[name]['velocity'] = self.joints[name][
                'controller'].getJointVelocity()
def main(args):

    pi = 3.1416
    equilibriums = (-0.75, -0.5)
    stiffnesses = (0, 0.25, 0.5, 0.75, 1)
    distances = range(7,21,1)

    with open('results/stiffness_test.csv', 'wb') as csvfile:

        writer = csv.writer(csvfile, delimiter=' ', quotechar='|', quoting=csv.QUOTE_MINIMAL)
        writer.writerow(['attempt', 'distance', 'stiffness', 'equilibirum', 'angle'])

        rospy.init_node('stiffnessTest', anonymous=True)
        
        upperarmRoll = DirectDrive("upperarm_roll", pi)
        elbow = Antagonist(-1, 1, -1, -1, 1, "biceps", "triceps", "elbow_encoder", 0, 2*pi, -0.75, 0.85)

        
        r = rospy.Rate(60)  
        
        print("Moving arm into place.")
        for i in range (0,300):
            upperarmRoll.servoTo(0)
            elbow.moveTo(0, 0)
            r.sleep()
        print("Test starting. Add weights.")
            
        for dist in distances:
            
            try:
                input("Adjust distance to " + str(dist) + " cm, and press enter.")
            except SyntaxError:
                pass
            
            for att in range (1,4):
                for stiff in stiffnesses:
                    for eq in equilibriums:
                        for i in range (0,200):
                            upperarmRoll.servoTo(0)
                            elbow.moveTo(eq, stiff)
                            r.sleep()
                        angle = elbow.getJointAngle()
                        writer.writerow([att, dist, stiff, eq, angle])
Example #5
0
class Gummi:
    def __init__(self):
        self.teleop = rospy.get_param("~teleop", 1)
        print(
            "Expecting teleoperation ('teleop' parameter in gummi.yaml file): "
            + str(self.teleop) + ".")

        self.pi = 3.1416
        self.initVariables()
        self.initJoints()
        self.initPublishers()
        self.initSubscribers()

    def initVariables(self):
        self.jointNames = ("shoulder_yaw", "shoulder_roll", "shoulder_pitch",
                           "upperarm_roll", "elbow", "forearm_roll",
                           "wrist_pitch", "hand_dof1")
        self.shoulderRollVel = 0
        self.shoulderPitchVel = 0
        self.shoulderYawVel = 0
        self.upperarmRollVel = 0
        self.elbowVel = 0
        self.forearmRollVel = 0
        self.wristVel = 0
        self.handDOF1Vel = 0
        self.shoulderRollCocont = 0.3
        self.shoulderPitchCocont = 0.3
        self.shoulderYawCocont = 0.3
        self.elbowCocont = 0.3
        self.wristCocont = 0.6

    def initJoints(self):
        self.shoulderYaw = Antagonist("shoulder_yaw")
        self.shoulderRoll = Antagonist("shoulder_roll")
        self.shoulderPitch = Antagonist("shoulder_pitch")
        self.upperarmRoll = DirectDrive("upperarm_roll", self.pi)
        self.elbow = Antagonist("elbow")
        self.forearmRoll = DirectDrive("forearm_roll", 1.75 * self.pi)
        self.wrist = Antagonist("wrist")
        self.headYaw = DirectDrive("head_yaw", 1.5 * self.pi)
        self.handDOF1 = DirectDrive("hand_dof1", 1.5 * self.pi)

    def initPublishers(self):
        self.jointStatePub = rospy.Publisher("gummi/joint_states",
                                             JointState,
                                             queue_size=10)

    def initSubscribers(self):
        rospy.Subscriber('gummi/joint_commands', JointState, self.cmdCallback)

    def cmdCallback(self, msg):
        if self.teleop == 1:
            self.setVelocity(msg.velocity)  #TODO: CHECK NAMES
            self.setCocontraction(msg.effort[0], msg.effort[1], msg.effort[2],
                                  msg.effort[4], msg.effort[6])
            self.doVelocityUpdate()
        else:
            self.setCocontraction(abs(msg.effort[0]), abs(msg.effort[1]),
                                  abs(msg.effort[2]), abs(msg.effort[4]),
                                  abs(msg.effort[6]))
            if (msg.effort[0] >= 0) and (msg.effort[1] >= 0) and (
                    msg.effort[2] >= 0) and (msg.effort[4] >=
                                             0) and (msg.effort[6] >= 0):
                self.servoTo(msg.position)  #TODO: CHECK NAMES
            else:
                self.passiveHold()

    def doUpdate(self):
        self.shoulderYaw.doUpdate()
        self.shoulderRoll.doUpdate()
        self.shoulderPitch.doUpdate()
        self.upperarmRoll.doUpdate()
        self.elbow.doUpdate()
        self.forearmRoll.doUpdate()
        self.wrist.doUpdate()
        self.handDOF1.doUpdate()

    def doVelocityUpdate(self):
        if self.shoulderYawCocont < 0:
            self.shoulderYaw.moveWith(self.shoulderYawVel,
                                      abs(self.shoulderYawCocont))
        else:
            self.shoulderYaw.servoWith(self.shoulderYawVel,
                                       self.shoulderYawCocont)
        if self.shoulderRollCocont < 0:
            self.shoulderRoll.moveWith(self.shoulderRollVel,
                                       abs(self.shoulderRollCocont))
        else:
            self.shoulderRoll.servoWith(self.shoulderRollVel,
                                        self.shoulderRollCocont)
        if self.shoulderPitchCocont < 0:
            self.shoulderPitch.moveWith(self.shoulderPitchVel,
                                        abs(self.shoulderPitchCocont))
        else:
            self.shoulderPitch.servoWith(self.shoulderPitchVel,
                                         self.shoulderPitchCocont)
        self.upperarmRoll.servoWith(self.upperarmRollVel)
        if self.elbowCocont < 0:
            self.elbow.moveWith(self.elbowVel, abs(self.elbowCocont))
        else:
            self.elbow.servoWith(self.elbowVel, self.elbowCocont)
        self.forearmRoll.servoWith(self.forearmRollVel)
        if self.wristCocont == -999:
            self.wrist.passiveHold(0.0)
        else:
            if self.wristCocont < 0:
                self.wrist.moveWith(self.wristVel, abs(self.wristCocont))
            else:
                self.wrist.servoWith(self.wristVel, self.wristCocont)
        self.handDOF1.servoWith(self.handDOF1Vel)

        self.publishJointState()

    def publishJointState(self):
        msg = JointState()
        msg.header.stamp = rospy.Time.now()
        msg.name = self.jointNames
        msg.position = self.getJointAngles()
        self.jointStatePub.publish(msg)

    def getJointAngles(self):
        angles = list()
        angles.append(self.shoulderYaw.getJointAngle())
        angles.append(self.shoulderRoll.getJointAngle())
        angles.append(self.shoulderPitch.getJointAngle())
        angles.append(self.upperarmRoll.getJointAngle())
        angles.append(self.elbow.getJointAngle())
        angles.append(self.forearmRoll.getJointAngle())
        angles.append(self.wrist.getJointAngle())
        angles.append(self.handDOF1.getJointAngle())
        return angles

    def setVelocity(self, velocities):
        self.shoulderYawVel = velocities[0]
        self.shoulderRollVel = velocities[1]
        self.shoulderPitchVel = velocities[2]
        self.upperarmRollVel = velocities[3]
        self.elbowVel = velocities[4]
        self.forearmRollVel = velocities[5]
        self.wristVel = velocities[6]
        self.handDOF1Vel = velocities[7]

    def setCocontraction(self, shoulderYaw, shoulderRoll, shoulderPitch, elbow,
                         wrist):
        self.shoulderYawCocont = shoulderYaw
        self.shoulderRollCocont = shoulderRoll
        self.shoulderPitchCocont = shoulderPitch
        self.elbowCocont = elbow
        self.wristCocont = wrist

    def servoTo(self, positions):
        if self.teleop == 0:
            self.shoulderYaw.servoTo(positions[0], self.shoulderYawCocont)
            self.shoulderRoll.servoTo(positions[1], self.shoulderRollCocont)
            self.shoulderPitch.servoTo(positions[2], self.shoulderPitchCocont)
            self.upperarmRoll.servoTo(positions[3])
            self.elbow.servoTo(positions[4], self.elbowCocont)
            self.forearmRoll.servoTo(positions[5])
            self.wrist.servoTo(positions[6], self.wristCocont)
            self.handDOF1.servoTo(positions[7])
            self.publishJointState()
        else:
            print(
                "WARNING: Asked to servo to pose, but ignoring as in teleop mode. Check gummi.yaml file."
            )

    def goTo(self, positions, now):
        if self.teleop == 0:
            self.shoulderYaw.goTo(positions[0], self.shoulderYawCocont, now)
            self.shoulderRoll.goTo(positions[1], self.shoulderRollCocont, now)
            self.shoulderPitch.goTo(positions[2], self.shoulderPitchCocont,
                                    now)
            self.upperarmRoll.servoTo(positions[3])
            self.elbow.goTo(positions[4], self.elbowCocont, now)
            self.forearmRoll.servoTo(positions[5])
            self.wrist.goTo(positions[6], self.wristCocont, now)
            self.handDOF1.servoTo(positions[7])
            self.publishJointState()
        else:
            print(
                "WARNING: Asked to go to pose, but ignoring as in teleop mode. Check gummi.yaml file."
            )

    def goRestingPose(self, now):
        self.shoulderYaw.goTo(0, self.shoulderYawCocont, now)
        self.shoulderRoll.goTo(0, self.shoulderRollCocont, now)
        self.shoulderPitch.goTo(0, self.shoulderPitchCocont, now)
        self.upperarmRoll.servoTo(0)
        self.elbow.goTo(0, self.elbowCocont, now)
        self.forearmRoll.servoTo(0)
        self.wrist.goTo(0, self.wristCocont, now)
        self.handDOF1.servoTo(0)

    def passiveHold(self):
        self.shoulderYaw.passiveHold(self.shoulderYawCocont)
        self.shoulderRoll.passiveHold(self.shoulderRollCocont)
        self.shoulderPitch.passiveHold(self.shoulderPitchCocont)
        self.upperarmRoll.doUpdate()
        self.elbow.passiveHold(self.elbowCocont)
        self.forearmRoll.doUpdate()
        self.wrist.passiveHold(self.wristCocont)
        self.handDOF1.doUpdate()

    def doGradualStartup(self):
        self.shoulderYaw.moveTo(-0.05, self.shoulderYawCocont)
        rospy.sleep(1)
        self.shoulderRoll.moveTo(-0.25, self.shoulderRollCocont)
        rospy.sleep(1)
        self.shoulderPitch.moveTo(0.25, self.shoulderPitchCocont)
        rospy.sleep(1)
        self.upperarmRoll.servoTo(0)
        rospy.sleep(1)
        self.elbow.moveTo(-0.6, self.elbowCocont)
        rospy.sleep(1)
        self.forearmRoll.servoTo(0)
        rospy.sleep(1)
        self.wrist.moveTo(0, self.wristCocont)
        rospy.sleep(1)
        self.handDOF1.servoTo(-2.2)
        rospy.sleep(1)
        self.headYaw.servoTo(0)
        rospy.sleep(1)

    def setCollisionResponses(self, shoulderYaw, shoulderRoll, shoulderPitch,
                              elbow, wrist):
        self.shoulderYaw.setCollisionResponse(shoulderYaw)
        self.shoulderRoll.setCollisionResponse(shoulderRoll)
        self.shoulderPitch.setCollisionResponse(shoulderPitch)
        self.elbow.setCollisionResponse(elbow)
        self.wrist.setCollisionResponse(wrist)

    def doZeroEquilibriumPose(self):
        self.shoulderYaw.moveTo(0.0, self.shoulderYawCocont)
        rospy.sleep(1)
        self.shoulderRoll.moveTo(0.0, self.shoulderRollCocont)
        rospy.sleep(1)
        self.shoulderPitch.moveTo(0.0, self.shoulderPitchCocont)
        rospy.sleep(1)
        self.upperarmRoll.servoTo(0)
        rospy.sleep(1)
        self.elbow.moveTo(0.0, self.elbowCocont)
        rospy.sleep(1)
        self.forearmRoll.servoTo(0)
        rospy.sleep(1)
        self.wrist.moveTo(0, self.wristCocont)
        rospy.sleep(1)
        self.handDOF1.servoTo(0.0)
        rospy.sleep(1)
        self.headYaw.servoTo(0)
Example #6
0
class Gummi:

    def __init__(self):
        self.pi = 3.1416
        self.initVariables()
        self.initJoints()
        self.initPublishers()
        self.initSubscribers()

    def initVariables(self):
        self.jointNames = ("shoulder_yaw", "shoulder_roll", "shoulder_pitch", "upperarm_roll", "elbow", "forearm_roll", "wrist_pitch")
        self.shoulderRollVel = 0
        self.shoulderPitchVel = 0
        self.shoulderYawVel = 0
        self.upperarmRollVel = 0
        self.elbowVel = 0
        self.forearmRollVel = 0
        self.wristVel = 0
        self.shoulderRollStiff = 0
        self.shoulderPitchStiff = 0
        self.shoulderYawStiff = 0.3
        self.elbowStiff = 0
        self.wristStiff = 0
        self.recordData = False

    def initJoints(self):
        self.shoulderYaw = Antagonist("shoulder_yaw")
        self.shoulderRoll = Antagonist("shoulder_roll")
        self.shoulderPitch = Antagonist("shoulder_pitch")
        self.upperarmRoll = DirectDrive("upperarm_roll", self.pi)
        self.elbow = Antagonist("elbow")
        self.forearmRoll = DirectDrive("forearm_roll", 1.75*self.pi)
        self.wrist = Antagonist("wrist")
        self.headYaw = DirectDrive("head_yaw", 1.5*self.pi)

    def initPublishers(self):
        self.jointStatePub = rospy.Publisher("gummi/joint_states", JointState,  queue_size=10) 

    def initSubscribers(self):
        rospy.Subscriber('gummi/joint_commands', JointState, self.cmdCallback)

    def cmdCallback(self, msg):
        self.setVelocity(msg.velocity) #TODO: CHECK NAMES
        self.setStiffness(msg.effort[0], msg.effort[1], msg.effort[2], msg.effort[4], msg.effort[6])
        self.doUpdate()

    def setMaxLoads(self, maxLoadShoulderYaw, maxLoadShoulderRoll, maxLoadShoulderPitch, maxLoadElbow, maxloadWrist):
        self.shoulderYaw.setMaxLoad(maxLoadShoulderYaw)
        self.shoulderRoll.setMaxLoad(maxLoadShoulderRoll)
        self.shoulderPitch.setMaxLoad(maxLoadShoulderPitch)
        self.elbow.setMaxLoad(maxLoadElbow)
        self.wrist.setMaxLoad(maxloadWrist)

    def doUpdate(self):
        if self.shoulderYawStiff < 0:
            self.shoulderYaw.moveWith(self.shoulderYawVel, abs(self.shoulderYawStiff))
        else:
            self.shoulderYaw.servoWith(self.shoulderYawVel, self.shoulderYawStiff)
        if self.shoulderRollStiff < 0:
            self.shoulderRoll.moveWith(self.shoulderRollVel, abs(self.shoulderRollStiff))
        else:
            self.shoulderRoll.servoWith(self.shoulderRollVel, self.shoulderRollStiff)
        if self.shoulderPitchStiff < 0:
            self.shoulderPitch.moveWith(self.shoulderPitchVel, abs(self.shoulderPitchStiff))
        else:
            self.shoulderPitch.servoWith(self.shoulderPitchVel, self.shoulderPitchStiff)
        self.upperarmRoll.servoWith(self.upperarmRollVel)
        if self.elbowStiff < 0:
            self.elbow.moveWith(self.elbowVel, abs(self.elbowStiff))
        else:
            self.elbow.servoWith(self.elbowVel, self.elbowStiff)
        self.forearmRoll.servoWith(self.forearmRollVel)
        if self.wristStiff == -999:
            self.wrist.passiveHold(0.0)
        else:
            if self.wristStiff < 0:
                self.wrist.moveWith(self.wristVel, abs(self.wristStiff))
            else:
                self.wrist.servoWith(self.wristVel, self.wristStiff)
        
        self.publishJointState()

        if self.recordData:
            self.addLineRecording()

    def publishJointState(self):
        msg = JointState()
        msg.header.stamp = rospy.Time.now()
        msg.name = self.jointNames
        msg.position = self.getJointAngles()
        msg.effort = self.getLoads()
        self.jointStatePub.publish(msg)

    def getJointAngles(self):
        angles = list()
        angles.append(self.shoulderYaw.getJointAngle())
        angles.append(self.shoulderRoll.getJointAngle())
        angles.append(self.shoulderPitch.getJointAngle())
        angles.append(self.upperarmRoll.getJointAngle())
        angles.append(self.elbow.getJointAngle())
        angles.append(self.forearmRoll.getJointAngle())
        angles.append(self.wrist.getJointAngle())
        return angles

    def setVelocity(self, velocities):
        self.shoulderYawVel = velocities[0]
        self.shoulderRollVel = velocities[1]
        self.shoulderPitchVel = velocities[2]
        self.upperarmRollVel = velocities[3]
        self.elbowVel = velocities[4]
        self.forearmRollVel = velocities[5]
        self.wristVel = velocities[6]

    def setStiffness(self, shoulderYaw, shoulderRoll, shoulderPitch, elbow, wrist):
        self.shoulderYawStiff = shoulderYaw
        self.shoulderRollStiff = shoulderRoll
        self.shoulderPitchStiff = shoulderPitch
        self.elbowStiff = elbow
        self.wristStiff = wrist

    def getLoads(self):
        loads = list()
        loads.append(self.shoulderYaw.getLoadRatio())
        loads.append(self.shoulderRoll.getLoadRatio())
        loads.append(self.shoulderPitch.getLoadRatio())
        loads.append(1234)
        loads.append(self.elbow.getLoadRatio())
        loads.append(1234)
        loads.append(self.wrist.getLoadRatio())
        return loads

    def goRestingPose(self):
        self.shoulderYaw.servoTo(0, self.shoulderYawStiff)
        self.shoulderRoll.servoTo(0, self.shoulderRollStiff)
        self.shoulderPitch.servoTo(0, self.shoulderPitchStiff)
        self.upperarmRoll.servoTo(0)
        self.elbow.servoTo(0, self.elbowStiff)
        self.forearmRoll.servoTo(0)
        self.wrist.servoTo(0, self.wristStiff)

    def doGradualStartup(self):
        self.shoulderYaw.moveTo(-0.1, self.shoulderYawStiff)
        rospy.sleep(1)
        self.shoulderRoll.moveTo(-1.25, self.shoulderRollStiff)
        rospy.sleep(1)
        self.shoulderPitch.moveTo(0.5, self.shoulderPitchStiff)
        rospy.sleep(1)
        self.upperarmRoll.servoTo(0)
        rospy.sleep(1)
        self.elbow.moveTo(0, self.elbowStiff)
        rospy.sleep(1)
        self.forearmRoll.servoTo(0)
        rospy.sleep(1)
        self.wrist.moveTo(0, self.wristStiff)
        rospy.sleep(1)
        self.headYaw.servoTo(0)
        rospy.sleep(1)

    def prepareRecording(self, fileNameBase):
        self.shoulderYaw.prepareRecording(fileNameBase)
        self.shoulderRoll.prepareRecording(fileNameBase)
        self.shoulderPitch.prepareRecording(fileNameBase)
        self.upperarmRoll.prepareRecording(fileNameBase)
        self.elbow.prepareRecording(fileNameBase)
        self.forearmRoll.prepareRecording(fileNameBase)
        self.wrist.prepareRecording(fileNameBase)
        
    def startRecording(self):
        self.timeStartRecording = rospy.Time.now()
        self.recordData = True

    def stopRecording(self):
        self.recordData = False

    def addLineRecording(self):        
        timeNow = rospy.Time.now()
        duration = timeNow - self.timeStartRecording
        delta = duration.to_sec()
        self.shoulderYaw.recordLine(delta)
        self.shoulderRoll.recordLine(delta)
        self.shoulderPitch.recordLine(delta)
        self.upperarmRoll.recordLine(delta)
        self.elbow.recordLine(delta)
        self.forearmRoll.recordLine(delta)
        self.wrist.recordLine(delta)
def main(args):

    pi = 3.1416

    rospy.init_node('stiffnessTest', anonymous=True)

    upperarmRoll = DirectDrive("upperarm_roll", pi)
    elbow = Antagonist(-1, 1, -1, -1, 1, "biceps", "triceps", "elbow_encoder",
                       0, 2 * pi, -0.75, 0.85)
    forearmRoll = DirectDrive("forearm_roll", 1.75 * pi)
    wrist = Antagonist(-1, -1, -1, -1, 1, "wrist_flexor", "wrist_extensor",
                       "wrist_encoder", 0, 1.75 * pi, -1.0, 1.0)

    r = rospy.Rate(60)

    for att in range(1, 11):

        print("Moving arm into place.")
        for i in range(0, 150):
            upperarmRoll.servoTo(0)
            forearmRoll.servoTo(0)
            wrist.servoTo(0, 0)
            elbow.moveTo(0, 0)
            r.sleep()
        print("Test started, attempt: " + str(att) + ".")

        fileName = 'results/step_test_stiffness_a_' + str(att) + '.csv'
        with open(fileName, 'wb') as csvfile:
            writer = csv.writer(csvfile,
                                delimiter=' ',
                                quotechar='|',
                                quoting=csv.QUOTE_MINIMAL)
            writer.writerow([
                'time', 'stiffness', 'angle', 'flexor-angle', 'extensor-angle'
            ])

            time1 = rospy.Time.now()
            for i in range(0, 400):
                upperarmRoll.servoTo(0)
                forearmRoll.servoTo(0)
                wrist.servoTo(0, 0)

                if i < 80:
                    stiff = 0.0
                else:
                    if i < 240:
                        stiff = 1.0
                    else:
                        stiff = 0.0

                elbow.moveTo(0, stiff)

                angle = elbow.getJointAngle() * 180 / pi
                flexor = joint.eqModel.flexor.getJointAngle() * 180 / pi
                extensor = joint.eqModel.extensor.getJointAngle() * 180 / pi
                time2 = rospy.Time.now()
                duration = time2 - time1
                delta = duration.to_sec()

                writer.writerow(
                    [delta, stiff, angle, flexorAngle, extensorAngle])
                r.sleep()