class MomentumKinematicsOptimizer(object):
def __init__(self):
self.q_init = None
self.dq_init = None
self.reg_orientation = 1e-2
self.reg_joint_position = 2.
self.joint_des = None
def reset(self):
self.kinematics_sequence = KinematicsSequence()
self.kinematics_sequence.resize(
self.planner_setting.get(PlannerIntParam_NumTimesteps),
self.planner_setting.get(PlannerIntParam_NumDofs))
def initialize(self,
planner_setting,
max_iterations=50,
eps=0.001,
endeff_traj_generator=None,
RobotWrapper=QuadrupedWrapper):
self.planner_setting = planner_setting
if endeff_traj_generator is None:
endeff_traj_generator = EndeffectorTrajectoryGenerator()
self.endeff_traj_generator = endeff_traj_generator
self.dt = planner_setting.get(PlannerDoubleParam_TimeStep)
self.num_time_steps = planner_setting.get(PlannerIntParam_NumTimesteps)
self.max_iterations = max_iterations
self.eps = eps
self.robot = RobotWrapper()
self.reset()
# Holds dynamics and kinematics results
self.com_dyn = np.zeros((self.num_time_steps, 3))
self.lmom_dyn = np.zeros((self.num_time_steps, 3))
self.amom_dyn = np.zeros((self.num_time_steps, 3))
self.com_kin = np.zeros((self.num_time_steps, 3))
self.lmom_kin = np.zeros((self.num_time_steps, 3))
self.amom_kin = np.zeros((self.num_time_steps, 3))
self.q_kin = np.zeros((self.num_time_steps, self.robot.model.nq))
self.dq_kin = np.zeros((self.num_time_steps, self.robot.model.nv))
self.hip_names = ['{}_HFE'.format(eff) for eff in self.robot.effs]
self.hip_ids = [
self.robot.model.getFrameId(name) for name in self.hip_names
]
self.eff_names = [
'{}_{}'.format(eff, self.robot.joints_list[-1])
for eff in self.robot.effs
]
self.inv_kin = PointContactInverseKinematics(self.robot.model,
self.eff_names)
self.motion_eff = {
'trajectory':
np.zeros((self.num_time_steps, 3 * self.inv_kin.ne)),
'velocity':
np.zeros((self.num_time_steps, 3 * self.inv_kin.ne)),
'trajectory_wrt_base':
np.zeros((self.num_time_steps, 3 * self.inv_kin.ne)),
'velocity_wrt_base':
np.zeros((self.num_time_steps, 3 * self.inv_kin.ne))
}
def fill_data_from_dynamics(self):
# The centroidal information
for it in range(self.num_time_steps):
self.com_dyn[it] = self.dynamic_sequence.dynamics_states[it].com
self.lmom_dyn[it] = self.dynamic_sequence.dynamics_states[it].lmom
self.amom_dyn[it] = self.dynamic_sequence.dynamics_states[it].amom
def fill_endeffector_trajectory(self):
self.endeff_pos_ref, self.endeff_vel_ref, self.endeff_contact = \
self.endeff_traj_generator(self)
def fill_kinematic_result(self, it, q, dq):
def framesPos(frames):
return np.vstack([data.oMf[idx].translation
for idx in frames]).reshape(-1)
def framesVel(frames):
return np.vstack([
self.inv_kin.get_world_oriented_frame_jacobian(q,
idx).dot(dq)[:3]
for idx in frames
]).reshape(-1)
data = self.inv_kin.robot.data
hg = self.inv_kin.robot.centroidalMomentum(q, dq)
# Storing on the internal array.
self.com_kin[it] = self.inv_kin.robot.com(q).T
self.lmom_kin[it] = hg.linear.T
self.amom_kin[it] = hg.angular.T
self.q_kin[it] = q.T
self.dq_kin[it] = dq.T
# The endeffector informations as well.
self.motion_eff['trajectory'][it] = framesPos(self.inv_kin.endeff_ids)
self.motion_eff['velocity'][it] = self.inv_kin.J[6:(self.inv_kin.ne +
2) * 3].dot(dq).T
self.motion_eff['trajectory_wrt_base'][it] = \
self.motion_eff['trajectory'][it] - framesPos(self.hip_ids)
self.motion_eff['velocity_wrt_base'][it] = \
self.motion_eff['velocity'][it] - framesVel(self.hip_ids)
# Storing on the kinematic sequence.
kinematic_state = self.kinematics_sequence.kinematics_states[it]
kinematic_state.com = self.com_kin[it]
kinematic_state.lmom = self.lmom_kin[it]
kinematic_state.amom = self.amom_kin[it]
kinematic_state.robot_posture.base_position = q[:3]
kinematic_state.robot_posture.base_orientation = q[3:7]
kinematic_state.robot_posture.joint_positions = q[7:]
kinematic_state.robot_velocity.base_linear_velocity = dq[:3]
kinematic_state.robot_velocity.base_angular_velocity = dq[3:6]
kinematic_state.robot_velocity.joint_velocities = dq[6:]
def optimize_initial_position(self, init_state):
# Optimize the initial configuration
q = se3.neutral(self.robot.model)
plan_joint_init_pos = self.planner_setting.get(
PlannerVectorParam_KinematicDefaultJointPositions)
if len(plan_joint_init_pos) != self.robot.num_ctrl_joints:
raise ValueError(
'Number of joints in config file not same as required for robot\n'
+ 'Got %d joints but robot expects %d joints.' %
(len(plan_joint_init_pos), self.robot.num_ctrl_joints))
q[7:] = np.matrix(plan_joint_init_pos).T
q[2] = self.robot.floor_height + 0.32 #was 0.32
#print q[2]
dq = np.matrix(np.zeros(self.robot.robot.nv)).T
com_ref = init_state.com
lmom_ref = np.zeros(3)
amom_ref = np.zeros(3)
endeff_pos_ref = np.array(
[init_state.effPosition(i) for i in range(init_state.effNum())])
endeff_vel_ref = np.matrix(np.zeros((init_state.effNum(), 3)))
endeff_contact = np.ones(init_state.effNum())
quad_goal = se3.Quaternion(
se3.rpy.rpyToMatrix(np.matrix([0.0, 0, 0.]).T))
q[3:7] = quad_goal.coeffs()
for iters in range(self.max_iterations):
# Adding small P controller for the base orientation to always start with flat
# oriented base.
quad_q = se3.Quaternion(float(q[6]), float(q[3]), float(q[4]),
float(q[5]))
amom_ref = 1e-1 * se3.log((quad_goal * quad_q.inverse()).matrix())
res = self.inv_kin.compute(q, dq, com_ref, lmom_ref, amom_ref,
endeff_pos_ref, endeff_vel_ref,
endeff_contact, None)
q = se3.integrate(self.robot.model, q, res)
if np.linalg.norm(res) < 1e-3:
print('Found initial configuration after {} iterations'.format(
iters + 1))
break
if iters == self.max_iterations - 1:
print('Failed to converge for initial setup.')
print("initial configuration: \n", q)
q[2] = 0.20
self.q_init = q.copy()
self.dq_init = dq.copy()
def optimize(self,
init_state,
contact_sequence,
dynamic_sequence,
plotting=False):
self.init_state = init_state
self.contact_sequence = contact_sequence
self.dynamic_sequence = dynamic_sequence
self.q_via = None
# Create array with centroidal and endeffector informations.
self.fill_data_from_dynamics()
self.fill_endeffector_trajectory()
# Run the optimization for the initial configuration only once.
if self.q_init is None:
self.optimize_initial_position(init_state)
# Get the desired joint trajectory
# print "num_joint_via:",self.planner_setting.get(PlannerIntParam_NumJointViapoints)
# print "joint_via:",self.planner_setting.get(PlannerCVectorParam_JointViapoints)
# TODO: this is for jump, should go to config file
# q_jump = [1., 0.1, -0.2 ,0.1, -0.2 ,-0.1, 0.2 ,-0.1, 0.2]
# q_via = np.matrix([.75, np.pi/2, -np.pi, np.pi/2, -np.pi, -np.pi/2, np.pi, -np.pi/2, np.pi]).T
# q_max = np.matrix([1.35, .7*np.pi/2, -.7*np.pi, .7*np.pi/2, -.7*np.pi, -.7*np.pi/2, .7*np.pi, -.7*np.pi/2, .7*np.pi]).T
# q_via0 = np.vstack((q_via.T, q_jump))
# self.q_via = np.vstack((q_via0, q_max.T))
joint_traj_gen = JointTrajectoryGenerator()
joint_traj_gen.num_time_steps = self.num_time_steps
joint_traj_gen.q_init = self.q_init[7:]
self.joint_des = np.zeros((len(self.q_init[7:]), self.num_time_steps),
float)
if self.q_via is None:
for i in range(self.num_time_steps):
self.joint_des[:, i] = self.q_init[7:].T
else:
joint_traj_gen.joint_traj(self.q_via)
for it in range(self.num_time_steps):
self.joint_des[:, it] = joint_traj_gen.eval_traj(it)
# Compute inverse kinematics over the full trajectory.
self.inv_kin.is_init_time = 0
q, dq = self.q_init.copy(), self.dq_init.copy()
for it in range(self.num_time_steps):
quad_goal = se3.Quaternion(
se3.rpy.rpyToMatrix(np.matrix([0.0, 0, 0.]).T))
quad_q = se3.Quaternion(float(q[6]), float(q[3]), float(q[4]),
float(q[5]))
amom_ref = (self.reg_orientation * se3.log(
(quad_goal * quad_q.inverse()).matrix()).T +
self.amom_dyn[it]).reshape(-1)
joint_regularization_ref = self.reg_joint_position * (
np.matrix(self.joint_des[:, it]).T - q[7:])
# joint_regularization_ref = self.reg_joint_position * (self.q_init[7 : ] - q[7 : ])
# Fill the kinematics results for it.
self.inv_kin.forward_robot(q, dq)
self.fill_kinematic_result(it, q, dq)
dq = self.inv_kin.compute(q, dq, self.com_dyn[it],
self.lmom_dyn[it], amom_ref,
self.endeff_pos_ref[it],
self.endeff_vel_ref[it],
self.endeff_contact[it],
joint_regularization_ref)
# Integrate to the next state.
q = se3.integrate(self.robot.model, q, dq * self.dt)
class MomentumKinematicsOptimizer(object):
def __init__(self):
self.q_init = None
self.dq_init = None
self.reg_orientation = 1e-2
self.reg_joint_position = 2.
self.joint_des = None
self.n_via_joint = 0
self.n_via_base = 0
self.via_joint = None
self.via_base = None
def reset(self):
self.kinematics_sequence = KinematicsSequence()
self.kinematics_sequence.resize(
self.planner_setting.get(PlannerIntParam_NumTimesteps),
self.planner_setting.get(PlannerIntParam_NumDofs))
def initialize(self,
planner_setting,
max_iterations=50,
eps=0.001,
endeff_traj_generator=None,
RobotWrapper=QuadrupedWrapper):
self.planner_setting = planner_setting
if endeff_traj_generator is None:
endeff_traj_generator = EndeffectorTrajectoryGenerator()
self.endeff_traj_generator = endeff_traj_generator
self.dt = planner_setting.get(PlannerDoubleParam_TimeStep)
self.num_time_steps = planner_setting.get(PlannerIntParam_NumTimesteps)
self.max_iterations = max_iterations
self.eps = eps
self.robot = RobotWrapper()
self.reset()
# Holds dynamics and kinematics results
self.com_dyn = np.zeros((self.num_time_steps, 3))
self.lmom_dyn = np.zeros((self.num_time_steps, 3))
self.amom_dyn = np.zeros((self.num_time_steps, 3))
self.com_kin = np.zeros((self.num_time_steps, 3))
self.lmom_kin = np.zeros((self.num_time_steps, 3))
self.amom_kin = np.zeros((self.num_time_steps, 3))
self.q_kin = np.zeros((self.num_time_steps, self.robot.model.nq))
self.dq_kin = np.zeros((self.num_time_steps, self.robot.model.nv))
self.hip_names = ['{}_HFE'.format(eff) for eff in self.robot.effs]
self.hip_ids = [
self.robot.model.getFrameId(name) for name in self.hip_names
]
self.eff_names = [
'{}_{}'.format(eff, self.robot.joints_list[-1])
for eff in self.robot.effs
]
self.inv_kin = PointContactInverseKinematics(self.robot.model,
self.eff_names)
self.snd_order_inv_kin = SecondOrderInverseKinematics(
self.robot.model, self.eff_names)
self.use_second_order_inv_kin = False
self.motion_eff = {
'trajectory':
np.zeros((self.num_time_steps, 3 * self.inv_kin.ne)),
'velocity':
np.zeros((self.num_time_steps, 3 * self.inv_kin.ne)),
'trajectory_wrt_base':
np.zeros((self.num_time_steps, 3 * self.inv_kin.ne)),
'velocity_wrt_base':
np.zeros((self.num_time_steps, 3 * self.inv_kin.ne))
}
def fill_data_from_dynamics(self):
# The centroidal information
for it in range(self.num_time_steps):
self.com_dyn[it] = self.dynamic_sequence.dynamics_states[it].com
self.lmom_dyn[it] = self.dynamic_sequence.dynamics_states[it].lmom
self.amom_dyn[it] = self.dynamic_sequence.dynamics_states[it].amom
def fill_endeffector_trajectory(self):
self.endeff_pos_ref, self.endeff_vel_ref, self.endeff_contact = \
self.endeff_traj_generator(self)
def fill_kinematic_result(self, it, q, dq):
def framesPos(frames):
return np.vstack([data.oMf[idx].translation
for idx in frames]).reshape(-1)
def framesVel(frames):
return np.vstack([
self.inv_kin.get_world_oriented_frame_jacobian(q,
idx).dot(dq)[:3]
for idx in frames
]).reshape(-1)
data = self.inv_kin.robot.data
hg = self.inv_kin.robot.centroidalMomentum(q, dq)
# Storing on the internal array.
self.com_kin[it] = self.inv_kin.robot.com(q).T
self.lmom_kin[it] = hg.linear.T
self.amom_kin[it] = hg.angular.T
self.q_kin[it] = q.T
self.dq_kin[it] = dq.T
# The endeffector informations as well.
self.motion_eff['trajectory'][it] = framesPos(self.inv_kin.endeff_ids)
self.motion_eff['velocity'][it] = self.inv_kin.J[6:(self.inv_kin.ne +
2) * 3].dot(dq).T
self.motion_eff['trajectory_wrt_base'][it] = \
self.motion_eff['trajectory'][it] - framesPos(self.hip_ids)
self.motion_eff['velocity_wrt_base'][it] = \
self.motion_eff['velocity'][it] - framesVel(self.hip_ids)
# Storing on the kinematic sequence.
kinematic_state = self.kinematics_sequence.kinematics_states[it]
kinematic_state.com = self.com_kin[it]
kinematic_state.lmom = self.lmom_kin[it]
kinematic_state.amom = self.amom_kin[it]
kinematic_state.robot_posture.base_position = q[:3]
kinematic_state.robot_posture.base_orientation = q[3:7]
kinematic_state.robot_posture.joint_positions = q[7:]
kinematic_state.robot_velocity.base_linear_velocity = dq[:3]
kinematic_state.robot_velocity.base_angular_velocity = dq[3:6]
kinematic_state.robot_velocity.joint_velocities = dq[6:]
def optimize_initial_position(self, init_state):
# Optimize the initial configuration
q = se3.neutral(self.robot.model)
plan_joint_init_pos = self.planner_setting.get(
PlannerVectorParam_KinematicDefaultJointPositions)
if len(plan_joint_init_pos) != self.robot.num_ctrl_joints:
raise ValueError(
'Number of joints in config file not same as required for robot\n'
+ 'Got %d joints but robot expects %d joints.' %
(len(plan_joint_init_pos), self.robot.num_ctrl_joints))
q[7:] = plan_joint_init_pos
q[2] = init_state.com[2]
dq = np.zeros(self.robot.robot.nv)
com_ref = init_state.com
lmom_ref = np.zeros(3)
amom_ref = np.zeros(3)
num_eff = len(self.eff_names)
endeff_pos_ref = np.array(
[init_state.effPosition(i) for i in range(num_eff)])
endeff_vel_ref = np.matrix(np.zeros((num_eff, 3)))
endeff_contact = np.ones(num_eff)
quad_goal = se3.Quaternion(se3.rpy.rpyToMatrix(np.zeros([
3,
])))
q[3:7] = quad_goal.coeffs()
for iters in range(self.max_iterations):
# Adding small P controller for the base orientation to always start with flat
# oriented base.
quad_q = se3.Quaternion(float(q[6]), float(q[3]), float(q[4]),
float(q[5]))
amom_ref = 1e-1 * se3.log((quad_goal * quad_q.inverse()).matrix())
res = self.inv_kin.compute(q, dq, com_ref, lmom_ref, amom_ref,
endeff_pos_ref, endeff_vel_ref,
endeff_contact, None)
q = se3.integrate(self.robot.model, q, res)
if np.linalg.norm(res) < 1e-3:
print('Found initial configuration after {} iterations'.format(
iters + 1))
break
if iters == self.max_iterations - 1:
print('Failed to converge for initial setup.')
print("initial configuration: \n", q)
self.q_init = q.copy()
self.dq_init = dq.copy()
def optimize(self,
init_state,
contact_sequence,
dynamic_sequence,
plotting=False):
self.init_state = init_state
self.contact_sequence = contact_sequence
self.dynamic_sequence = dynamic_sequence
# Create array with centroidal and endeffector informations.
self.fill_data_from_dynamics()
self.fill_endeffector_trajectory()
# Run the optimization for the initial configuration only once.
if self.q_init is None:
self.optimize_initial_position(init_state)
# Generate smooth joint trajectory for regularization
self.joint_des = np.zeros((len(self.q_init[7:]), self.num_time_steps),
float)
if self.n_via_joint == 0:
for i in range(self.num_time_steps):
self.joint_des[:, i] = self.q_init[7:].T
else:
joint_traj_gen = TrajectoryInterpolator()
joint_traj_gen.num_time_steps = self.num_time_steps
joint_traj_gen.init = self.q_init[7:]
joint_traj_gen.end = self.q_init[7:]
joint_traj_gen.generate_trajectory(self.n_via_joint,
self.via_joint, self.dt)
for it in range(self.num_time_steps):
self.joint_des[:, it] = joint_traj_gen.evaluate_trajecory(it)
# Generate smooth base trajectory for regularization
self.base_des = np.zeros((3, self.num_time_steps), float)
if self.n_via_base == 0:
for it in range(self.num_time_steps):
self.base_des[:, it] = np.array([0., 0., 0.]).reshape(-1)
else:
base_traj_gen = TrajectoryInterpolator()
base_traj_gen.num_time_steps = self.num_time_steps
base_traj_gen.init = np.array([0.0, 0.0, 0.0])
base_traj_gen.end = np.array([0.0, 0.0, 0.0])
base_traj_gen.generate_trajectory(self.n_via_base, self.via_base,
self.dt)
for it in range(self.num_time_steps):
self.base_des[:, it] = base_traj_gen.evaluate_trajecory(it)
# Compute inverse kinematics over the full trajectory.
self.inv_kin.is_init_time = 0
q, dq = self.q_init.copy(), self.dq_init.copy()
if self.use_second_order_inv_kin:
q_kin, dq_kin, com_kin, lmom_kin, amom_kin, endeff_pos_kin, endeff_vel_kin = \
self.snd_order_inv_kin.solve(self.dt, q, dq, self.com_dyn, self.lmom_dyn,
self.amom_dyn, self.endeff_pos_ref, self.endeff_vel_ref,
self.endeff_contact, self.joint_des.T, self.base_des.T)
for it, (q, dq) in enumerate(zip(q_kin, dq_kin)):
self.inv_kin.forward_robot(q, dq)
self.fill_kinematic_result(it, q, dq)
else:
for it in range(self.num_time_steps):
quad_goal = se3.Quaternion(
se3.rpy.rpyToMatrix(np.matrix(self.base_des[:, it]).T))
quad_q = se3.Quaternion(float(q[6]), float(q[3]), float(q[4]),
float(q[5]))
## check if this instance belongs to the flight phase and set the momentums accordingly
for eff in range(len(self.eff_names)):
if self.dynamic_sequence.dynamics_states[it].effActivation(
eff):
is_flight_phase = False
break
else:
is_flight_phase = True
## for flight phase keep the desired momentum constant
if is_flight_phase:
lmom_ref[0:2] = mom_ref_flight[0:2]
amom_ref = mom_ref_flight[3:6]
lmom_ref[2] -= self.planner_setting.get(
PlannerDoubleParam_RobotWeight) * self.dt
else:
lmom_ref = self.lmom_dyn[it]
amom_ref = (self.reg_orientation * se3.log(
(quad_goal * quad_q.inverse()).matrix()).T +
self.amom_dyn[it]).reshape(-1)
joint_regularization_ref = self.reg_joint_position * (
self.joint_des[:, it] - q[7:])
# Fill the kinematics results for it.
self.inv_kin.forward_robot(q, dq)
self.fill_kinematic_result(it, q, dq)
# Store the momentum to be used in flight phase
if not is_flight_phase:
mom_ref_flight = (
self.inv_kin.J[0:6, :].dot(dq)).reshape(-1)
# Compute the inverse kinematics
dq = self.inv_kin.compute(q, dq, self.com_dyn[it], lmom_ref,
amom_ref, self.endeff_pos_ref[it],
self.endeff_vel_ref[it],
self.endeff_contact[it],
joint_regularization_ref,
is_flight_phase)
# Integrate to the next state.
q = se3.integrate(self.robot.model, q, dq * self.dt)