def setJointPosition(self, robot, position, kp=1.0, kv=0.3): import pybullet as p num_joints = p.getNumJoints(robot) zero_vec = [0.0] * num_joints if len(position) == num_joints: p.setJointMotorControlArray(robot, range(num_joints), p.POSITION_CONTROL, targetPositions=position, targetVelocities=zero_vec, positionGains=[kp] * num_joints, velocityGains=[kv] * num_joints)
def applyAction(self, actions): forces = [0.] * len(self.motors) for m in range(len(self.motors)): forces[m] = self.motor_power[m]*actions[m]*0.082 p.setJointMotorControlArray(self.human, self.motors,controlMode=p.TORQUE_CONTROL, forces=forces) p.stepSimulation() time.sleep(0.01) distance=5 yaw = 0
def setJointPosition(robot, position, kp=1.0, kv=0.3): num_joints = p.getNumJoints(robot) zero_vec = [0.0] * num_joints if len(position) == num_joints: p.setJointMotorControlArray(robot, range(num_joints), p.POSITION_CONTROL, targetPositions=position, targetVelocities=zero_vec, positionGains=[kp] * num_joints, velocityGains=[kv] * num_joints) else: print("Not setting torque. " "Expected torque vector of " "length {}, got {}".format(num_joints, len(torque)))
def take_step(self, action, robot_arm='left', gains=0.05, forces=1, step_sim=True): action = np.clip(action, a_min=self.action_space.low, a_max=self.action_space.high) # print('cameraYaw=%.2f, cameraPitch=%.2f, distance=%.2f' % p.getDebugVisualizerCamera(physicsClientId=self.id)[-4:-1]) # print('Total time:', self.total_time) # self.total_time += 0.1 self.iteration += 1 if self.last_sim_time is None: self.last_sim_time = time.time() action *= 0.05 action_robot = action indices = self.robot_left_arm_joint_indices if robot_arm == 'left' else self.robot_right_arm_joint_indices if robot_arm == 'right' else self.robot_both_arm_joint_indices robot_joint_states = p.getJointStates(self.robot, jointIndices=indices, physicsClientId=self.id) robot_joint_positions = np.array([x[0] for x in robot_joint_states]) for _ in range(self.frame_skip): action_robot[robot_joint_positions + action_robot < self.robot_lower_limits] = 0 action_robot[robot_joint_positions + action_robot > self.robot_upper_limits] = 0 robot_joint_positions += action_robot p.setJointMotorControlArray(self.robot, jointIndices=indices, controlMode=p.POSITION_CONTROL, targetPositions=robot_joint_positions, positionGains=np.array( [gains] * self.action_robot_len), forces=[forces] * self.action_robot_len, physicsClientId=self.id) self.setup_record_video() # pierre if step_sim: # Update robot position for _ in range(self.frame_skip): p.stepSimulation(physicsClientId=self.id) self.update_targets() if self.gui: # Slow down time so that the simulation matches real time self.slow_time() self.record_video_frame()
def _sendPositionCommand(self, commands): '''''' num_motors = len(self.arm_joint_indices) pb.setJointMotorControlArray(self.id, self.arm_joint_indices, pb.POSITION_CONTROL, commands, targetVelocities=[0.] * num_motors, forces=[self.max_force] * num_motors, positionGains=[self.position_gain] * num_motors, velocityGains=[1.0] * num_motors)
def set_pos(set_mode, position_list=[]): maxForces = [10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10] posGain = [0.01] * 12 #velGain = [0.1] * 12 p.setJointMotorControlArray( quadruped, jointIndices=motor_id_list, controlMode=set_mode, targetPositions=position_list, #positionGains = posGain, #velocityGains = velGain, forces=maxForces)
def step(self): # while True: currj = [ p.getJointState(self.body, i)[0] for i in range(self.n_joints) ] indj = [6, 3, 8, 5, 10] targj = [currj[1], -currj[1], -currj[1], currj[1], currj[1]] p.setJointMotorControlArray(self.body, indj, p.POSITION_CONTROL, targj, positionGains=np.ones(5))
def setJointPosition(self, robot, position, kp=1.0, kv=0.3): import pybullet as p num_joints = p.getNumJoints(robot) zero_vec = [0.0] * num_joints if len(position) == num_joints: p.setJointMotorControlArray(robot, range(num_joints), p.POSITION_CONTROL, targetPositions=position, targetVelocities=zero_vec, positionGains=[kp] * num_joints, velocityGains=[kv] * num_joints)
def set_actuator_postions(self, joint_angles): serial_joint_angles = self.parallel_to_serial_joint_angles( joint_angles) pybullet.setJointMotorControlArray( bodyUniqueId=self.model[1], jointIndices=self.joint_indices, controlMode=pybullet.POSITION_CONTROL, targetPositions=list(serial_joint_angles.T.reshape(12)), positionGains=[self.kp] * 12, velocityGains=[self.kv] * 12, forces=[self.max_torque] * 12, )
def pos(x1, y1, z1, roll0, yaw0, pitch0, robotID, eefID): flag = True # xin = p.addUserDebugParameter("x", -2, 2, x) # yin = p.addUserDebugParameter("y", -2, 2, y) # zin = p.addUserDebugParameter("z", -2, 2, z) # roll1 = p.addUserDebugParameter("roll", -2*pi, 2*pi, roll) # yaw1 = p.addUserDebugParameter("yaw", -2*pi, 2*pi, yaw) # pitch1 = p.addUserDebugParameter("pitch", -2*pi, 2*pi, pitch) # capture1 = p.addUserDebugParameter("capture",-3,3,capture) while (flag): # print('looping',flag) # capture = p.readUserDebugParameter(capture1) # ur5_camera(robotID) # x = p.readUserDebugParameter(xin) # y = p.readUserDebugParameter(yin) # z = p.readUserDebugParameter(zin) # roll = p.readUserDebugParameter(roll1) # yaw = p.readUserDebugParameter(yaw1) # pitch = p.readUserDebugParameter(pitch1) x = x1 y = y1 z = z1 roll = roll0 yaw = yaw0 pitch = pitch0 robotEndOrn = p.getQuaternionFromEuler([roll, yaw, pitch]) jointPose = p.calculateInverseKinematics(robotID, eefID, [x, y, z], robotEndOrn) # print("---------------jointPose-------------------------") # print(jointPose) joint_index = [1, 2, 3, 4, 5, 6] pose_group = [ jointPose[0], jointPose[1], jointPose[2], jointPose[3], jointPose[4], jointPose[5] ] p.setJointMotorControlArray(robotID, joint_index, p.POSITION_CONTROL, targetPositions=pose_group) rXYZ = p.getLinkState(robotID, eefID)[-2] # real XYZ rWxWyWz = p.getLinkState(robotID, eefID)[-1] # print(x,y,z,rXYZ,rWxWyWz) Te = list(map(ABSE, [x, y, z], rXYZ)) #translation error Qe = list(map(ABSE, robotEndOrn, rWxWyWz)) #rotation error # print("x_err= {:.3f}, y_err= {:.3f}, z_err= {:.3f}".format(*Te)) # print("W1_err= {:.3f}, W2_err= {:.3f}, W3_err= {:.3f}, W4_err= {:.3f}".format(*Qe)) # for x in Te: # print(x) if (check(Te, Qe)): # print('Te,Qe = ',Te,Qe) flag = False time.sleep(0.05) p.stepSimulation()
def move_to_initial_position(self): if self.save_sequence: self.traj_indices.append(len(self.data)) pos_left = [2.5, 0.75, -0.4, 0, 0, 3.14] pos_right = [-2.5, 0.75, -0.4, 0, 0, 3.14] pb.setJointMotorControlArray(self.robot, self.joint_indices_left, pb.POSITION_CONTROL, targetPositions=pos_left, physicsClientId=self.pb_client_id) pb.setJointMotorControlArray(self.robot, self.joint_indices_right, pb.POSITION_CONTROL, targetPositions=pos_right, physicsClientId=self.pb_client_id) for s in range(100): pb.stepSimulation(physicsClientId=self.pb_client_id) if self.delay: time.sleep(self.dt)
def set_joint_torques(side, torques): if SIMULATION == True: if side == Side.LEFT: joints = [0, 1, 2, 3, 4] if side == Side.RIGHT: joints = [5, 6, 7, 8, 9] for x in torques: x = min(40, max(-40, x)) p.setJointMotorControlArray(bipedId, joints, p.TORQUE_CONTROL, forces=torques)
def set_joint_torque(self, tau): """ sets joint torques to the values specified by the 1D numpy array tau """ assert (tau.shape[0] == self.num_joints) zero_gains = tau.shape[0] * (0., ) p.setJointMotorControlArray(self.robot_id, self.joint_ids, p.TORQUE_CONTROL, forces=tau, positionGains=zero_gains, velocityGains=zero_gains)
def __init__(self, robot_id, pinocchio_robot, joint_names, endeff_names, useFixedBase=False): self.nq = pinocchio_robot.nq self.nv = pinocchio_robot.nv self.nj = len(joint_names) self.nf = len(endeff_names) self.robot_id = robot_id self.pinocchio_robot = pinocchio_robot self.useFixedBase = useFixedBase self.joint_names = joint_names self.endeff_names = endeff_names bullet_joint_map = {} for ji in range(p.getNumJoints(robot_id)): bullet_joint_map[p.getJointInfo(robot_id, ji)[1].decode('UTF-8')] = ji self.bullet_joint_ids = np.array( [bullet_joint_map[name] for name in joint_names]) self.pinocchio_joint_ids = np.array( [pinocchio_robot.model.getJointId(name) for name in joint_names]) self.pin2bullet_joint_only_array = [] if not self.useFixedBase: for i in range(2, self.nj + 2): self.pin2bullet_joint_only_array.append( np.where(self.pinocchio_joint_ids == i)[0][0]) else: for i in range(1, self.nj + 1): self.pin2bullet_joint_only_array.append( np.where(self.pinocchio_joint_ids == i)[0][0]) # Disable the velocity control on the joints as we use torque control. p.setJointMotorControlArray(robot_id, self.bullet_joint_ids, p.VELOCITY_CONTROL, forces=np.zeros(self.nj)) # In pybullet, the contact wrench is measured at a joint. In our case # the joint is fixed joint. Pinocchio doesn't add fixed joints into the joint # list. Therefore, the computation is done wrt to the frame of the fixed joint. self.bullet_endeff_ids = [ bullet_joint_map[name] for name in endeff_names ] self.pinocchio_endeff_ids = [ pinocchio_robot.model.getFrameId(name) for name in endeff_names ]
def send_joint_command(self, tau): # TODO: Apply the torques on the base towards the simulator as well. if not self.useFixedBase: assert(tau.shape[0] == self.nv - 6) else: assert(tau.shape[0] == self.nv) zeroGains = tau.shape[0] * (0.,) p.setJointMotorControlArray(self.robot_id, self.bullet_joint_ids, p.TORQUE_CONTROL, forces=tau[self.pin2bullet_joint_only_array], positionGains=zeroGains, velocityGains=zeroGains)
def _assign_throttle(self, ee_target_pos, position_gain): # Calculate joint positions using inverse kinematics joint_pos = p.calculateInverseKinematics(self.botId, 6, ee_target_pos) p.setJointMotorControlArray(bodyIndex=self.botId, jointIndices=range(self.numJoints), controlMode=p.POSITION_CONTROL, targetPositions=joint_pos, forces=[300] * self.numJoints, positionGains=[position_gain] * self.numJoints, targetVelocities=[0.0] * self.numJoints, velocityGains=[1] * self.numJoints)
def _send_move_command(self, jointPoses): num_arm_indices = len(self.armInds) p.setJointMotorControlArray( self.armId, self.armInds, controlMode=p.POSITION_CONTROL, targetPositions=jointPoses[:num_arm_indices], targetVelocities=[0] * num_arm_indices, # forces=[self.MAX_FORCE] * num_arm_indices, forces=[100, 100, 60, 60, 50, 40, 40], positionGains=[0.3] * num_arm_indices, velocityGains=[1] * num_arm_indices)
def setController(self, robotID, controller_type, joint_indices, targetPositions=[None, None], targetVelocities=None, targetTorques=None): if controller_type == 'velocity': if targetPositions[0] != None: p.setJointMotorControlArray( robotID, joint_indices, p.VELOCITY_CONTROL, targetPositions=targetPositions, targetVelocities=targetVelocities, velocityGains=np.array( [1, 1, 1, 1, 1, 1, 1])) #, forces = [50]*len(targetVelocities)) else: p.setJointMotorControlArray( robotID, joint_indices, p.VELOCITY_CONTROL, targetVelocities=targetVelocities ) #, velocityGains = np.array([1,1,1,1,1,1,1])*1) elif controller_type == 'position': p.setJointMotorControlArray(robotID, joint_indices, p.POSITION_CONTROL, targetPositions=targetPositions, targetVelocities=targetVelocities) elif controller_type == 'torque': if not self.torque_control_mode: p.setJointMotorControlArray(robotID, joint_indices, p.VELOCITY_CONTROL, forces=[0] * len(joint_indices)) #TODO: add a routine to read link indices to change this. BUG? for link_idx in joint_indices: p.changeDynamics(robotID, link_idx, linearDamping=0.0, angularDamping=0.0, jointDamping=0.0) self.torque_control_mode = True p.setJointMotorControlArray(robotID, joint_indices, p.TORQUE_CONTROL, forces=targetTorques) else: print("unknown controller type")
def front_grasp(botId, rightCoords, leftCoords, data, steps=10, tableId1=None, tableId2=None, both=None, primitive_name=None, front_grasp=False): revoluteJoints = getRevoluteJoints(botId) x_prev_left = p.getLinkState(botId, 28)[0][0] iter_left_x = (rightCoords[0]-x_prev_left)/steps x_prev_right = p.getLinkState(botId, 51)[0][0] iter_right_x = (rightCoords[0]-x_prev_right)/steps y_prev_left = p.getLinkState(botId, 28)[0][1] iter_left_y = (leftCoords[1]-y_prev_left)/steps y_prev_right = p.getLinkState(botId, 51)[0][1] iter_right_y = (rightCoords[1]-y_prev_right)/steps i = 0 for i in range(steps): x_prev_left += iter_left_x x_prev_right += iter_right_x y_prev_left += iter_left_y y_prev_right += iter_right_y gripperPosition1 = p.calculateInverseKinematics(botId, 28, [x_prev_left, y_prev_left, leftCoords[2]], maxNumIterations=1000) gripperPosition2 = p.calculateInverseKinematics(botId, 51, [x_prev_right, y_prev_right, rightCoords[2]], maxNumIterations=1000) jointStates = getJointStates(botId) get_primitive_data(botId, data, primitive_name, sequence=1, tableId1=tableId1, tableId2=tableId2, front_grasp=front_grasp, both=both) p.setJointMotorControlArray(botId, jointIndices=revoluteJoints[0:10], controlMode=p.POSITION_CONTROL, targetPositions=gripperPosition1[0:10]) p.setJointMotorControlArray(botId, jointIndices=revoluteJoints[10:], controlMode=p.POSITION_CONTROL, targetPositions=gripperPosition2[10:]) for i in range(10): p.stepSimulation() get_primitive_data(botId, data, primitive_name, sequence=2, tableId1=tableId1, tableId2=tableId2, front_grasp=front_grasp, both=both) jointStates = getJointStates(botId) return gripperPosition1, gripperPosition2
def configure_simulation(dt, enableGUI): global jointTorques # Load the robot for Pinocchio solo = robots_loader.loadSolo(True) solo.initViewer(loadModel=True) # Start the client for PyBullet if enableGUI: physicsClient = p.connect(p.GUI) else: physicsClient = p.connect(p.DIRECT) # p.GUI for graphical version # p.DIRECT for non-graphical version # Set gravity (disabled by default) p.setGravity(0, 0, -9.81) # Load horizontal plane for PyBullet p.setAdditionalSearchPath(pybullet_data.getDataPath()) planeId = p.loadURDF("plane.urdf") # Load the robot for PyBullet robotStartPos = [0, 0, 0.35] robotStartOrientation = p.getQuaternionFromEuler([0, 0, 0]) p.setAdditionalSearchPath( "/opt/openrobots/share/example-robot-data/robots/solo_description/robots" ) robotId = p.loadURDF("solo.urdf", robotStartPos, robotStartOrientation) # Set time step of the simulation p.setTimeStep(dt) # Disable default motor control for revolute joints revoluteJointIndices = [0, 1, 3, 4, 6, 7, 9, 10] p.setJointMotorControlArray( robotId, jointIndices=revoluteJointIndices, controlMode=p.VELOCITY_CONTROL, targetVelocities=[0.0 for m in revoluteJointIndices], forces=[0.0 for m in revoluteJointIndices]) # Enable torque control for revolute joints jointTorques = [0.0 for m in revoluteJointIndices] p.setJointMotorControlArray(robotId, revoluteJointIndices, controlMode=p.TORQUE_CONTROL, forces=jointTorques) # Compute one step of simulation for initialization p.stepSimulation() return robotId, solo, revoluteJointIndices
def PDcontrol(q, qdot): qError = desired_q - q qdotError = desired_qdot - qdot Kp = np.diagflat(kps) Kd = np.diagflat(kds) forces = Kp.dot(qError) + Kd.dot(qdotError) force1 = np.clip(forces[0], -maxF[0], maxF[0]) force2 = np.clip(forces[1], -maxF[0], maxF[1]) forces = [force1, force2] p.setJointMotorControlArray(pole, [0, 1], controlMode=p.TORQUE_CONTROL, forces=forces)
def __disable_pybullet_velocity_control(self): """ To disable the high friction velocity motors created by default at all revolute and prismatic joints while loading them from the urdf. """ pybullet.setJointMotorControlArray( bodyUniqueId=self.finger_id, jointIndices=self.pybullet_joint_indices, controlMode=pybullet.VELOCITY_CONTROL, targetVelocities=[0] * len(self.pybullet_joint_indices), forces=[0] * len(self.pybullet_joint_indices), )
def foot_position2motor_angle(self, foot_positions): serial_joint_angles = kinematics.serial_quadruped_inverse_kinematics( foot_positions, self.sim_hardware_config) # print(serial_joint_angles) pybullet.setJointMotorControlArray( bodyUniqueId=self.body_id, jointIndices=self.joint_indices, controlMode=pybullet.POSITION_CONTROL, targetPositions=list(serial_joint_angles.T.reshape(12)), positionGains=[self.motor_kp] * 12, velocityGains=[self.motor_kv] * 12, forces=[self.motor_max_torque] * 12, )
def set_q_pb(self, qd): """ Get the joint configuration of the robot arm. Args: qd (list): list of desired joint position """ pybullet.setJointMotorControlArray( bodyUniqueId=self._arm_id, jointIndices=range(0, self._num_joints), controlMode=pybullet.POSITION_CONTROL, targetPositions=qd, physicsClientId=self.physics_id) return qd
def set_motor_pos_vel(robot, joint_id, pos_cmd, vel_cmd): pos_applied = OrderedDict() vel_applied = OrderedDict() for (joint_name, pos_des), (_, vel_des) in zip(pos_cmd.items(), vel_cmd.items()): pos_applied[joint_id[joint_name]] = pos_des vel_applied[joint_id[joint_name]] = vel_des p.setJointMotorControlArray(robot, pos_applied.keys(), controlMode=p.POSITION_CONTROL, targetPositions=list(pos_applied.values()), targetVelocities=list(vel_applied.values()))
def moveToPos(kukaID, finalPos, finalOrn, k=500): num_joints = p.getNumJoints(kukaID) finalAngles = p.calculateInverseKinematics(kukaID, 6, finalPos, finalOrn) targetAngles = getAngleInterpolation(kukaID, finalAngles, k) for i in range(k): p.setJointMotorControlArray(bodyUniqueId=kukaID, jointIndices=range(num_joints), controlMode=p.POSITION_CONTROL, targetPositions=targetAngles[i], forces=num_joints * [100]) for j in range(ceil(500 / k)): p.stepSimulation() time.sleep(1. / 240.)
def simulateDyn(self, actions): if self.SIM_READDY == 1: p.setJointMotorControlArray(self.body_id, self.ACTUATED_JOINTS_INDEX, p.TORQUE_CONTROL, forces=actions, physicsClientId=self.client_id) p.stepSimulation(physicsClientId=self.client_id) if self.LOGDATA == 1: self.actions_seq.append(actions) self.observeState() else: raise Exception("Simulation not set up")
def applyAction(self, actions): forces = [0.] * len(self.motors) for m in range(len(self.motors)): forces[m] = self.motor_power[m] * actions[m] * 0.082 p.setJointMotorControlArray(self.human, self.motors, controlMode=p.TORQUE_CONTROL, forces=forces) p.stepSimulation() time.sleep(0.01) distance = 5 yaw = 0
def setJointPosition(self, position, kp=1.0, kv=1.0): print('Joint position controller') zero_vec = [0.0] * len(self.controllable_joints) p.setJointMotorControlArray( self.robot_id, self.controllable_joints, p.POSITION_CONTROL, targetPositions=position, targetVelocities=zero_vec, positionGains=[kp] * len(self.controllable_joints), velocityGains=[kv] * len(self.controllable_joints)) for _ in range(100): # to settle the robot to its position p.stepSimulation()
def send_joint_torque(self, tau): """ sends torque commands (needs an array of desired joint torques) """ assert (tau.shape[0] == self.nj) zeroGains = tau.shape[0] * (0., ) p.setJointMotorControlArray(self.robotId, self.bullet_joint_ids, p.TORQUE_CONTROL, forces=tau, positionGains=zeroGains, velocityGains=zeroGains)
def hold(flag): if flag: curr_joint_value[7] = 1 curr_joint_value[8] = 1 else: curr_joint_value[7] = 0 curr_joint_value[8] = 0 p.setJointMotorControlArray(kukaId, range(11), p.POSITION_CONTROL, targetPositions=curr_joint_value) time.sleep(1)
def full_code(): p.connect(p.GUI) p.setGravity(0, 0, -9.8) _ = p.loadURDF(os.path.join(MODELS_DIR, PLANE_URDF)) r2d2 = p.loadURDF(os.path.join(MODELS_DIR, R2D2_URDF), 0, 0, 0.5) p.setJointMotorControlArray(r2d2, R2D2_WHEEL_JOINTS, p.VELOCITY_CONTROL, [1, 1, 1, 1], [1, 1, 1, 1]) for ts in xrange(10000): print(ts) for i in xrange(p.getNumJoints(r2d2)): print(i, p.getJointState(r2d2, i)) p.stepSimulation() time.sleep(.0001)
def apply_action_ik(target_ee_pos, target_ee_quat, target_gripper_state, robot_id, end_effector_index, movable_joints, lower_limit, upper_limit, rest_pose, joint_range, num_sim_steps=5): joint_poses = p.calculateInverseKinematics(robot_id, end_effector_index, target_ee_pos, target_ee_quat, lowerLimits=lower_limit, upperLimits=upper_limit, jointRanges=joint_range, restPoses=rest_pose, jointDamping=[0.001] * len(movable_joints), solver=0, maxNumIterations=100, residualThreshold=.01) p.setJointMotorControlArray( robot_id, movable_joints, controlMode=p.POSITION_CONTROL, targetPositions=joint_poses, # targetVelocity=0, forces=[5] * len(movable_joints), positionGains=[0.03] * len(movable_joints), # velocityGain=1 ) # set gripper action p.setJointMotorControl2(robot_id, movable_joints[-2], controlMode=p.POSITION_CONTROL, targetPosition=target_gripper_state[0], force=500, positionGain=0.03) p.setJointMotorControl2(robot_id, movable_joints[-1], controlMode=p.POSITION_CONTROL, targetPosition=target_gripper_state[1], force=500, positionGain=0.03) for _ in range(num_sim_steps): p.stepSimulation()
id_robot = bullet.loadURDF("TwoJointRobot_w_fixedJoints.urdf", robot_base, robot_orientation, useFixedBase=True) else: id_revolute_joints = [0, 1] id_robot = bullet.loadURDF("TwoJointRobot_wo_fixedJoints.urdf", robot_base, robot_orientation, useFixedBase=True) bullet.changeDynamics(id_robot,-1,linearDamping=0, angularDamping=0) bullet.changeDynamics(id_robot,0,linearDamping=0, angularDamping=0) bullet.changeDynamics(id_robot,1,linearDamping=0, angularDamping=0) jointTypeNames = ["JOINT_REVOLUTE", "JOINT_PRISMATIC","JOINT_SPHERICAL","JOINT_PLANAR","JOINT_FIXED","JOINT_POINT2POINT","JOINT_GEAR"] # Disable the motors for torque control: bullet.setJointMotorControlArray(id_robot, id_revolute_joints, bullet.VELOCITY_CONTROL, forces=[0.0, 0.0]) # Target Positions: start = 0.0 end = 1.0 steps = int((end-start)/delta_t) t = [0]*steps q_pos_desired = [[0.]* steps,[0.]* steps] q_vel_desired = [[0.]* steps,[0.]* steps] q_acc_desired = [[0.]* steps,[0.]* steps] for s in range(steps): t[s] = start+s*delta_t q_pos_desired[0][s] = 1./(2.*math.pi) * math.sin(2. * math.pi * t[s]) - t[s] q_pos_desired[1][s] = -1./(2.*math.pi) * (math.cos(2. * math.pi * t[s]) - 1.0)