def create_torque_trajectory_generator(dt, robot):
N_CONFIG = N_JOINTS + 6
jtg = NdTrajectoryGenerator("torqueTrajGen")
jtg.initial_value.value = [0.] * N_CONFIG
jtg.trigger.value = 1.0
jtg.init(dt, N_CONFIG)
return jtg
def create_config_trajectory_generator(dt, robot):
N_CONFIG = N_JOINTS + 6
jtg = NdTrajectoryGenerator("jtg")
jtg.initial_value.value = robot.device.state.value
jtg.trigger.value = 1.0
jtg.init(dt, N_CONFIG)
return jtg
def create_zmp_trajectory_generator(dt, robot):
comTrajGen = NdTrajectoryGenerator("zmpTrajGen")
zmp = list(robot.dynamic.com.value)
zmp[2] = 0.0
comTrajGen.initial_value.value = zmp
comTrajGen.trigger.value = 1.0
comTrajGen.init(dt, 3)
return comTrajGen
def create_orientation_rpy_trajectory_generator(dt, robot, signal_name):
trajGen = NdTrajectoryGenerator(signal_name + "OrientationTrajGen")
M = robot.dynamic.signal(signal_name).value
v = list(rotation_matrix_to_rpy(np.array(M)[:3, :3]))
trajGen.initial_value.value = v
trajGen.trigger.value = 1.0
trajGen.init(dt, 3)
return trajGen
def create_position_trajectory_generator(dt, robot, signal_name):
trajGen = NdTrajectoryGenerator(signal_name + "PosTrajGen")
M = robot.dynamic.signal(signal_name).value
v = [M[i][3] for i in range(3)]
trajGen.initial_value.value = v
trajGen.trigger.value = 1.0
trajGen.init(dt, 3)
return trajGen
def create_pose_rpy_trajectory_generator(dt, robot, signal_name):
trajGen = NdTrajectoryGenerator(signal_name + "TrajGen")
M = robot.dynamic.signal(signal_name).value
pos = [M[i][3] for i in range(3)]
euler = list(rotation_matrix_to_rpy(np.array(M)[:3, :3]))
v = pos + euler
trajGen.initial_value.value = v
trajGen.trigger.value = 1.0
trajGen.init(dt, 6)
return trajGen
def create_scalar_trajectory_generator(dt, init_value, name):
trajGen = NdTrajectoryGenerator(name)
trajGen.initial_value.value = [init_value]
trajGen.trigger.value = 1.0
trajGen.init(dt, 1)
return trajGen
def create_com_trajectory_generator(dt, robot):
comTrajGen = NdTrajectoryGenerator("comTrajGen")
comTrajGen.initial_value.value = robot.dynamic.com.value
comTrajGen.trigger.value = 1.0
comTrajGen.init(dt, 3)
return comTrajGen
def create_joint_trajectory_generator(dt, robot):
jtg = NdTrajectoryGenerator("jtg")
jtg.initial_value.value = robot.device.state.value[6:]
jtg.trigger.value = 1.0
jtg.init(dt, N_JOINTS)
return jtg
def create_force_traj_gen(name, initial_value, dt):
force_traj_gen = NdTrajectoryGenerator(name)
force_traj_gen.initial_value.value = initial_value
force_traj_gen.init(dt, 6)
return force_traj_gen
def create_com_traj_gen(conf, dt):
com_traj_gen = NdTrajectoryGenerator("com_traj_gen")
com_traj_gen.initial_value.value = conf.COM_DES
com_traj_gen.init(dt, 3)
return com_traj_gen