Project/trajectory.py
def trajectoryUpAndDown(robot, length=500, dist=0.2, orientation=True):
    """
    Generate a trajectory of cartesian coordinates of an up and down movement

    :param robot: Robot object
    :param length: number of positions
    :param dist: height of the step / distance on z
    :param orientation: if true generate xyzrpy coordinates, else generate xyz coordinates
    :return: List of cartesian coordinates
    """
    pin.forwardKinematics(robot.model, robot.data, robot.motors)
    pin.framesForwardKinematics(robot.model, robot.data, robot.motors)

    X0 = np.array([])
    if orientation:
        vect = [0, 0, ((2 * dist) / length), 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
                0, 0]
        if robot.simu:
            for i in robot.URDFframes:
                X0 = np.hstack((X0, p.getLinkState(robot.id, i)[:2][0]))
                X0 = np.hstack((X0, p.getLinkState(robot.id, i)[:2][1][:-1]))
        else:
            for i in robot.pinFrames:
                X0 = np.hstack((X0, robot.getFrameXYZRPY(i)))
    else:
        vect = [0, 0, ((2 * dist) / length), 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
        if robot.simu:
            for i in robot.URDFframes:
                X0 = np.hstack((X0, p.getLinkState(robot.id, i)[0]))
        else:
            for frame in robot.pinFrames:
                X0 = np.hstack((X0, robot.data.oMf[frame].translation))

    traj = [X0]
    for i in range(length // 2):
        next_X = list(map(operator.sub, traj[-1], vect))
        traj.append(next_X)
    for j in range(length // 2):
        next_X = list(map(operator.add, traj[-1], vect))
        traj.append(next_X)

    return traj

    Generate a trajectory of cartesian coordinates of an up and down movement

    :param robot: Robot object
    :param length: number of positions
    :param dist: height of the step / distance on z
    :param orientation: if true generate xyzrpy coordinates, else generate xyz coordinates
    :return: List of cartesian coordinates
    
Generate a trajectory of cartesian coordinates of an up and down movement
Generate a trajectory of cartesian coordinates of an up and down movement
def trajectoryUpAndDown(robot, length=500, dist=0.2, orientation=True):
    pin.forwardKinematics(robot.model, robot.data, robot.motors)
    pin.framesForwardKinematics(robot.model, robot.data, robot.motors)
    X0 = np.array([])
    if orientation:
        vect = [0, 0, ((2 * dist) / length), 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
                0, 0]
        if robot.simu:
            for i in robot.URDFframes:
                X0 = np.hstack((X0, p.getLinkState(robot.id, i)[:2][0]))
                X0 = np.hstack((X0, p.getLinkState(robot.id, i)[:2][1][:-1]))
        else:
            for i in robot.pinFrames:
                X0 = np.hstack((X0, robot.getFrameXYZRPY(i)))
    else:
        vect = [0, 0, ((2 * dist) / length), 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
        if robot.simu:
            for i in robot.URDFframes:
                X0 = np.hstack((X0, p.getLinkState(robot.id, i)[0]))
        else:
            for frame in robot.pinFrames:
                X0 = np.hstack((X0, robot.data.oMf[frame].translation))
    traj = [X0]
    for i in range(length // 2):
        next_X = list(map(operator.sub, traj[-1], vect))
        traj.append(next_X)
    for j in range(length // 2):
        next_X = list(map(operator.add, traj[-1], vect))
        traj.append(next_X)
    return traj
def trajectoryUpAndDown ( robot , length = 500 , dist = 0.2 , orientation = True ) : pin . forwardKinematics ( robot . model , robot . data , robot . motors ) pin . framesForwardKinematics ( robot . model , robot . data , robot . motors ) X0 = np . array ( [ ] ) if orientation : vect = [ 0 , 0 , ( ( 2 * dist ) / length ) , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 ] if robot . simu : for i in robot . URDFframes : X0 = np . hstack ( ( X0 , p . getLinkState ( robot . id , i ) [ : 2 ] [ 0 ] ) ) X0 = np . hstack ( ( X0 , p . getLinkState ( robot . id , i ) [ : 2 ] [ 1 ] [ : - 1 ] ) ) else : for i in robot . pinFrames : X0 = np . hstack ( ( X0 , robot . getFrameXYZRPY ( i ) ) ) else : vect = [ 0 , 0 , ( ( 2 * dist ) / length ) , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 ] if robot . simu : for i in robot . URDFframes : X0 = np . hstack ( ( X0 , p . getLinkState ( robot . id , i ) [ 0 ] ) ) else : for frame in robot . pinFrames : X0 = np . hstack ( ( X0 , robot . data . oMf [ frame ] . translation ) ) traj = [ X0 ] for i in range ( length // 2 ) : next_X = list ( map ( operator . sub , traj [ - 1 ] , vect ) ) traj . append ( next_X ) for j in range ( length // 2 ) : next_X = list ( map ( operator . add , traj [ - 1 ] , vect ) ) traj . append ( next_X ) return traj
Generate a trajectory of cartesian coordinates of an up and down movement
Generate a trajectory of cartesian coordinates of an up and down movement
"""
    Generate a trajectory of cartesian coordinates of an up and down movement

    :param robot: Robot object
    :param length: number of positions
    :param dist: height of the step / distance on z
    :param orientation: if true generate xyzrpy coordinates, else generate xyz coordinates
    :return: List of cartesian coordinates
    """
List of cartesian coordinates
List of cartesian coordinates
robot
None
length
number of positions
number of positions
dist
height of the step / distance on z
height of the step / distance on z
orientation
if true generate xyzrpy coordinates, else generate xyz coordinates
if true generate xyzrpy coordinates else generate xyz coordinates