CODEWITHSUNDEEP
pythonnao-robot
marc nicole
marc nicole

Reputation: 83

Arms effector does'nt seem to be moving correcly using Transformer()

I want to use Tranformer class to make the arms move in the cartesian reference based on this: http://doc.aldebaran.com/2-5/naoqi/motion/control-cartesian.html. I have tried many parameters across the reference and rotations but still the arms doesn't move but a litte. Here's my code to move upward the RArm:

def update_transform(self, effector, frame, d_vector):

    currentTf = almath.Transform(self.motion.getTransform(effector, frame, False))
    
    target1Tf = almath.Transform(currentTf.r1_c4, currentTf.r2_c4, currentTf.r3_c4)
     
    target1Tf *= almath.Transform(d_vector["dx"], 0.0, 0.0)
    target1Tf *= almath.Transform(0.0, d_vector["dy"], 0.0)
    target1Tf *= almath.Transform(0.0, 0.0, d_vector["dz"])
    target1Tf *= almath.Transform().fromRotY(d_vector["dwy"])
    target1Tf *= almath.Transform().fromRotX(d_vector["dwx"])
    target1Tf *= almath.Transform().fromRotZ(d_vector["dwz"])
    
    return currentTf,target1Tf

then in the run() method:

self.motion.wakeUp()
    
# Send robot to Stand Init
self.posture.goToPosture("StandInit", 0.8)

effectorList = ["RArm"]
frame = motion.FRAME_WORLD
path_list = []
d_vector_l = {"dx": 0.01, "dy": 0.02, "dz": 0.03 , "dwx": 0.05 * almath.TO_RAD, "dwy": -85.02 * almath.TO_RAD,
            "dwz": 0.05 * almath.TO_RAD}
currentTfL, target1TfL = self.update_transform(effectorList[0], frame, d_vector_l)
path_list.append(list(target1TfL.toVector()))
time_list = [[2]]
axis_mask_list = [motion.AXIS_MASK_ALL]

self.motion.transformInterpolations(effectorList, frame, path_list, axis_mask_list, time_list)

time.sleep(4.0)

self.motion.rest()

Any clues why?

Upvotes: 0

Views: 24

Answers (0)

Related Questions