def setupJointLimitTask(): featureJl = FeatureJointLimits('featureJl') featureJl.actuate() dyn = robot.dynamic plug(dyn.signal('position'), featureJl.signal('joint')) plug(dyn.signal('upperJl'), featureJl.signal('upperJl')) plug(dyn.signal('lowerJl'), featureJl.signal('lowerJl')) taskJl = Task('taskJl') taskJl.add('featureJl') taskJl.signal('controlGain').value = .1
taskRightArm = Task('taskRightArm') taskRightArm.add('featureRightArm') taskRightArm.signal('controlGain').value = 1. # --- Task LeftArm for blocking the left arm featureLeftArm = FeatureGeneric('featureLeftArm') featureLeftArm.signal('jacobianIN').value = reduce(lambda jac, i : jac + ((29+i)*(0.,)+(1.,)+(6-i)*(0.,),), range(7), ()) featureLeftArm.signal('errorIN').value = 7*(0,) taskLeftArm = Task('taskLeftArm') taskLeftArm.add('featureLeftArm') taskLeftArm.signal('controlGain').value = 1 # --- Joint Limits featureJl = FeatureJointLimits('featureJl') featureJl.actuate() plug(dyn.signal('position'), featureJl.signal('joint')) plug(dyn.signal('upperJl'), featureJl.signal('upperJl')) plug(dyn.signal('lowerJl'), featureJl.signal('lowerJl')) taskJl = Task('taskJl') taskJl.add('featureJl') taskJl.signal('controlGain').value = .1 # --- LEGS featureLegs = FeatureGeneric('featureLegs') jacobianLegs = MatrixConstant('jacobianLegs') jac = 12*[dimension*[0.]] jac[0][6] = 1. jac[1][7] = 1. jac[2][8] = 1. jac[3][9] = 1. jac[4][10] = 1.
taskRightArm.signal('controlGain').value = 1. # --- Task LeftArm for blocking the left arm featureLeftArm = FeatureGeneric('featureLeftArm') featureLeftArm.signal('jacobianIN').value = reduce( lambda jac, i: jac + ((29 + i) * (0., ) + (1., ) + (6 - i) * (0., ), ), range(7), ()) featureLeftArm.signal('errorIN').value = 7 * (0, ) taskLeftArm = Task('taskLeftArm') taskLeftArm.add('featureLeftArm') taskLeftArm.signal('controlGain').value = 1 # --- Joint Limits featureJl = FeatureJointLimits('featureJl') featureJl.actuate() plug(dyn.signal('position'), featureJl.signal('joint')) plug(dyn.signal('upperJl'), featureJl.signal('upperJl')) plug(dyn.signal('lowerJl'), featureJl.signal('lowerJl')) taskJl = Task('taskJl') taskJl.add('featureJl') taskJl.signal('controlGain').value = .1 # --- LEGS featureLegs = FeatureGeneric('featureLegs') jacobianLegs = MatrixConstant('jacobianLegs') jac = 12 * [dimension * [0.]] jac[0][6] = 1. jac[1][7] = 1. jac[2][8] = 1. jac[3][9] = 1. jac[4][10] = 1.