def DiscreteFiniteHorizonLQR(system,
X0,
DT,
Q=None,
R=None,
tol=1e-6,
steps=1000,
Pk=None):
mvi = trep.MidpointVI(system)
dsys = discopt.DSystem(mvi, [0, DT])
# create an A and B:
Xt = np.vstack((X0, X0))
Ut = np.array([X0[2:4]])
A, B = dsys.linearize_trajectory(Xt, Ut)
A = A[0]
B = B[0]
# now we can start solving Riccati equation:
if Pk is None:
Pk = Q
Pkp1 = 1000 * Pk
err = True
for i in range(steps):
Pkp1 = Q + MM(A.T, Pk, A) - MM(A.T, Pk, B,
np.linalg.inv(
(R + MM(B.T, Pk, B))), B.T, Pk, A)
if np.linalg.norm(Pkp1 - Pk, ord=2) < tol:
err = False
break
Pk = Pkp1
# calc K
K = MM(np.linalg.inv(R + MM(B.T, Pk, B)), B.T, Pk, A)
return err, K, Pk
def LQROptimizer(system, X0, DT, Q=None, R=None, tol=1e-6, steps=1000):
"""
This class takes a system, an equilibrium state, a timestep, weighting
matrices, an error tolerance. It then solves a discrete-time LQR problem
to build a feedback regulator about the equilibrium point.
"""
# first let's build an MVI and dsystem
mvi = trep.MidpointVI(system)
tvec = np.arange(0.0, steps * DT, DT) #np.array([0, DT])
dsys = discopt.DSystem(mvi, tvec)
# build reference trajectory:
Xd = np.array([X0] * len(tvec))
Ud = np.array([X0[system.nQd:system.nQ]] * (len(tvec) - 1))
# build cost matrices:
if Q is None: Q = np.identity(dsys.nX)
if R is None: R = np.identity(dsys.nU)
Qk = lambda k: Q
Rk = lambda k: R
(Kstab, A, B) = dsys.calc_feedback_controller(Xd,
Ud,
Qk,
Rk,
return_linearization=True)
err = False
if np.linalg.norm(Kstab[0] - Kstab[1], ord=2) > tol:
rospy.logwarn("LQR stabilizing controller not settled!")
err = True
return err, Kstab[0]
def main():
# first let's build a system:
sys = DeltaRobotTrep(L_base, L_platform, R_base, R_platform)
dt = 0.05
tf = 10.0
# setup initial conditions
_ = sys.forward(*(-0.2 * np.ones(3)))
_ = sys.inverse(0.0, 0.0, 1.2)
q0 = sys.q
# add external force:
trep.forces.BodyWrench(sys, sys.platform_center,
(0, 0, "external_z", 0, 0, 0))
def f_func(t):
if t > 3.0:
return 50
else:
return 0
# amp = 20
# period = 1.0
# return amp*np.sin(t*2*pi/period) + 40
mvi = trep.MidpointVI(sys)
mvi.initialize_from_configs(0.0, q0, dt, q0)
q = [mvi.q2]
t = [mvi.t2]
while mvi.t1 < tf:
mvi.step(mvi.t2 + dt, u1=[f_func(mvi.t2)])
q.append(mvi.q2)
t.append(mvi.t2)
sys = DeltaRobotTrep(L_base, L_platform, R_base, R_platform)
visual.visualize_3d([visual.VisualItem3D(sys, t, q)])
def simulate_system(system):
system.satisfy_constraints(tolerance=1e-1, keep_kinematic=True)
q0 = system.q
tcur = 0.0
mvi = trep.MidpointVI(system)
mvi.initialize_from_configs(tcur, q0, dt, q0)
T = [mvi.t1]
Q = [mvi.q1]
steps = 0
while mvi.t1 < tf:
# system.satisfy_constraints(tolerance=1e-1, keep_kinematic=True)
# if mvi.t1 < (400*click):
# u = tuple([-19*kt, 19*kt])
# else:
# u = tuple([0,0])
u = tuple([0 * kt, 0 * kt])
mvi.step(mvi.t2 + dt, u, (), 5000)
T.append(mvi.t1)
Q.append(mvi.q1)
steps = steps + 1
# while tcur < tf
# stance for 100 clicks
# flight
# make sure tf is ~0.2 s
return (T, Q)
def simulate_system(system):
"""
Simulates the system from its current configuration with no
initial velocity for the duration and time step specified by global variables.
"""
# Extract the current configuration into a tuple to use as the two
# initial configurations of the variational integrator.
q0 = system.q
# Create and initialize the variational integrator.
mvi = trep.MidpointVI(system)
mvi.initialize_from_configs(0.0, q0, dt, q0)
# Run the simulation and save the results into the lists 't' and 'q'.
q = [mvi.q1]
t = [mvi.t1]
while mvi.t1 < tf:
mvi.step(mvi.t2 + dt)
q.append(mvi.q1)
t.append(mvi.t1)
# Print out progress during the simulation.
if abs(mvi.t1 - round(mvi.t1)) < dt / 2.0:
print "t =", mvi.t1
return (t, q)
def simulate_system(system, motor_angles):
system.satisfy_constraints(tolerance=1e-1, keep_kinematic=True)
q0 = system.q
tcur = 0.0
mvi = trep.MidpointVI(system)
mvi.initialize_from_configs(tcur, q0, dt, q0)
T = [mvi.t1]
Q = [mvi.q1]
steps = 0
while mvi.t1 < tf:
system.satisfy_constraints(tolerance=1e-1, keep_kinematic=True)
mvi.step(mvi.t2 + dt, tuple([0, 0]), tuple(motor_angles[steps, :]),
5000)
T.append(mvi.t1)
Q.append(mvi.q1)
steps = steps + 1
# TODO: determine force necessary to make the change specified by the change in motor angles
# TODO: make motor1 a kinematic variable, that can be determined from system dynamics?
return (T, Q)
def main(links, dt, tf, generalized_method, hybrid_wrenched):
# Create the system and integrator
system = make_pendulum(links, hybrid_wrenched)
mvi = trep.MidpointVI(system)
# Generate a reference trajectory
(t_ref, q_ref, u_ref) = generate_reference(mvi, dt, tf)
write_csv('ref.csv', t_ref, u_ref)
# Uncomment to display the reference trajectory
#visualize(system, t_ref, q_ref, u_ref)
# Perform inverse dynamics
if generalized_method:
(t_sol, u_sol) = inverse_dynamics_generalized(mvi, t_ref, q_ref)
else:
(t_sol, u_sol) = inverse_dynamics_specialized(mvi, t_ref, q_ref)
write_csv('sol.csv', t_sol, u_sol)
# Simulate the system with the new inputs
(t, q, u) = simulate_system(mvi, t_sol, u_sol, q_ref[0], q_ref[1])
# Uncomment to display the reconstructed trajectory
#visualize(system, t, q, u)
verify_solution(q_ref, q)
def __init__(self, *args, **kwargs):
self.sys = kwargs.pop('sys', None)
if self.sys == None:
print "Must give trep system as kwarg!"
return
super(VI_EKF, self).__init__(*args, **kwargs)
self.mvi = trep.MidpointVI(self.sys)
self.dsys = discopt.DSystem(self.mvi, array([0, self.Dt]))
def __init__(self, system, t, beta=0.7, tolerance=1e-1, DT=None):
# create a VI
self.mvi = trep.MidpointVI(system)
# create a dsys object:
self.dsys = discopt.DSystem(self.mvi, t)
# set default optimization props:
self.beta = beta
self.tolerance = tolerance
if DT:
self.DT = DT
else:
self.DT = t[1] - t[0]
# setup parameters for estimating whether we have time to take a step:
N = 3
self.tsamps = deque([self.DT / 3.0] * N, maxlen=N)
self.coeffs = [2, 4, 10]
def create_system(joint_forces=False, string_forces=False,
string_constraints=False, config_trajectory=False):
# Creates the mechanical system, variational integrator, and discrete system
# Creates a desired trajectory suitable for the mechanical system from the ik data
system = Puppet(joint_forces=joint_forces,
string_forces=string_forces,
string_constraints=string_constraints)
if not os.path.isfile('puppet-desired.mat'):
generate_desired_trajectory()
(t,Qd,p,v,u,rho) = trep.load_trajectory('puppet-desired.mat', system)
# Disregard all data except t,Q
dsys = discopt.DSystem(trep.MidpointVI(system), t)
xid = dsys.build_trajectory(Qd)
return dsys, xid
def generate_vi_sim(system, q0):
# Create and initialize the variational integrator
mvi = trep.MidpointVI(system)
mvi.initialize_from_configs(0.0, q0, dt, q0)
# This is our simulation loop. We save the results in two lists.
q = [mvi.q2]
t = [mvi.t2]
while mvi.t1 < tf:
qk2 = qk_func(system, mvi.t2)
try:
mvi.step(mvi.t2 + dt, (), qk2)
except trep.ConvergenceError:
print "VI integration failed at", mvi.t2 + dt, "seconds"
break
q.append(mvi.q2)
t.append(mvi.t2)
return t, q
def simulate_system(system):
# Now we'll extract the current configuration into a tuple to use as
# initial conditions for a variational integrator.
q0 = system.q
# Create and initialize the variational integrator
mvi = trep.MidpointVI(system)
mvi.initialize_from_configs(0.0, q0, dt, q0)
# This is our simulation loop. We save the results in two lists.
q = [mvi.q2]
t = [mvi.t2]
while mvi.t1 < tf:
mvi.step(mvi.t2 + dt)
q.append(mvi.q2)
t.append(mvi.t2)
return (t, q)
def generate_desired_trajectory():
puppet = trep.puppets.Puppet(joint_forces=False, string_forces=False,
string_constraints=True)
puppet.q = {
'torso_rx' : -0.05,
#'torso_ry' : 0.1,
'torso_tz' : 0.0,
'lelbow_rx' : 1.57,
'relbow_rx' : 1.57,
'lhip_rx' : pi/2 - 0.6,
'rhip_rx' : pi/2 - 0.6,
'lknee_rx' : -pi/2 + 0.6,
'rknee_rx' : -pi/2 + 0.6,
## 'Left Arm String' : 15.4,
## 'Right Arm String' : 15.4,
## 'Left Leg String' : 16.6,
## 'Right Leg String' : 16.6
}
puppet.project_string_controls()
q0 = puppet.q
u0 = tuple([0.0]*len(puppet.inputs))
qk2_0 = puppet.qk
dt = 0.01
# Create and initialize a variational integrator for the system.
mvi = trep.MidpointVI(puppet)
mvi.initialize_from_configs(0.0, q0, dt, q0)
#print qk2
left_leg_index = puppet.get_config('left_leg_string-length').k_index
right_leg_index = puppet.get_config('right_leg_string-length').k_index
left_arm_index = puppet.get_config('left_arm_string-length').k_index
right_arm_index = puppet.get_config('right_arm_string-length').k_index
def calc_vk():
v2 = [(q2-q1)/(mvi.t2 - mvi.t1) for (q2, q1) in zip(mvi.q2, mvi.q1)]
v2 = v2[len(puppet.dyn_configs):]
return v2
q = [mvi.q2]
p = [mvi.p2]
v = [calc_vk()]
rho = []
t = [mvi.t2]
while mvi.t1 < 10.0:
# For our kinematic inputs, we first copy our starting position.
qk2 = list(qk2_0)
# Now we want to move some of the strings up and down.
qk2[left_leg_index] += -0.1*sin(0.6*pi*mvi.t1)
qk2[right_leg_index] += 0.1*sin(0.6*pi*mvi.t1)
qk2[left_arm_index] += 0.1*sin(0.6*pi*mvi.t1)
qk2[right_arm_index] += -0.1*sin(0.6*pi*mvi.t1)
qk2 = tuple(qk2)
rho.append(qk2)
mvi.step(mvi.t2+dt, u0, qk2)
q.append(mvi.q2)
p.append(mvi.p2)
v.append(calc_vk())
t.append(mvi.t2)
# The puppet can take a while to simulate, so print out the time
# occasionally to indicate our progress.
if abs(mvi.t2 - round(mvi.t2)) < dt/2.0:
print "t =",mvi.t2
t = np.array(t)
q = np.array(q)
p = np.array(p)
v = np.array(v)
rho = np.array(rho)
trep.system.save_trajectory('puppet-desired.mat',
puppet, t, q, p, v, None, rho)
def QuadMain():
quadcm_m = 1.0
quadmotor_m = 1.0
ball_m = 2.5
dt = 0.01 # timestep set to 10ms
# Create a new trep system - 6 generalized coordinates for quadrotor: x,y,z,roll,pitch,yaw
system = trep.System()
trep.potentials.Gravity(system, name="Gravity")
quadz = trep.Frame(system.world_frame, trep.TZ, "quadz")
quady = trep.Frame(quadz, trep.TY, "quady")
quadx = trep.Frame(quady, trep.TX, "quadx")
quadrx = trep.Frame(quadx, trep.RX, "quadrx", kinematic=True)
quadry = trep.Frame(quadrx, trep.RY, "quadry", kinematic=True)
# quadrz = trep.Frame(quadry,trep.RZ,"quadrz")
quadx.set_mass(quadcm_m) # central point mass
# # define motor positions and mass
# quad1 = trep.Frame(quadrz,trep.CONST_SE3,((1,0,0),(0,1,0),(0,0,1),(3,0,0)),"quad1")
# quad1.set_mass(quadmotor_m)
# quad2 = trep.Frame(quadrz,trep.CONST_SE3,((1,0,0),(0,1,0),(0,0,1),(-3,0,0)),"quad2")
# quad2.set_mass(quadmotor_m)
# quad3 = trep.Frame(quadrz,trep.CONST_SE3,((1,0,0),(0,1,0),(0,0,1),(0,3,0)),"quad3")
# quad3.set_mass(quadmotor_m)
# quad4 = trep.Frame(quadrz,trep.CONST_SE3,((1,0,0),(0,1,0),(0,0,1),(0,-3,0)),"quad4")
# quad4.set_mass(quadmotor_m)
# define ball frame
ballrx = trep.Frame(quadx, trep.RX, "ballrx", kinematic=False)
ballry = trep.Frame(ballrx, trep.RY, "ballry", kinematic=False)
ball = trep.Frame(ballry, trep.CONST_SE3,
((1, 0, 0), (0, 1, 0), (0, 0, 1), (0, 0, -1)), "ballcm")
ball.set_mass(ball_m)
# set thrust vector with input u1
trep.forces.BodyWrench(system,
quadry, (0, 0, 'u1', 0, 0, 0),
name='wrench1')
# # set four thrust vectors with inputs u1,u2,u3,u4
# trep.forces.BodyWrench(system,quad1,(0,0,'u1',0,0,0),name='wrench1')
# trep.forces.BodyWrench(system,quad2,(0,0,'u2',0,0,0),name='wrench2')
# trep.forces.BodyWrench(system,quad3,(0,0,'u3',0,0,0),name='wrench3')
# trep.forces.BodyWrench(system,quad4,(0,0,'u4',0,0,0),name='wrench4')
# set quadrotor initial altitude at 3m
system.get_config("quadz").q = 3.0
# Now we'll extract the current configuration into a tuple to use as
# initial conditions for a variational integrator.
q0 = system.q
# Create and initialize the variational integrator
mvi = trep.MidpointVI(system)
mvi.initialize_from_configs(0.0, q0, dt, q0)
# These are our simulation variables.
q = mvi.q2
t = mvi.t2
# u0=tuple([(4*quadmotor_m+quadcm_m+ball_m)/4*9.8,(4*quadmotor_m+quadcm_m+ball_m)/4*9.8,(4*quadmotor_m+quadcm_m+ball_m)/4*9.8,(4*quadmotor_m+quadcm_m+ball_m)/4*9.8])
# u=[u0]
u0 = np.array([(quadcm_m + ball_m) * 9.8])
u = tuple(u0)
# start ROS node to broadcast transforms to rviz
rospy.init_node('quad_simulator')
broadcaster = tf.TransformBroadcaster()
pub = rospy.Publisher('config', Float32MultiArray)
statepub = rospy.Publisher('joint_states', JointState)
jointstates = JointState()
configs = Float32MultiArray()
# subscribe to joystick topic from joy_node
rospy.Subscriber("joy", Joy, joycall)
r = rospy.Rate(100) # simulation rate set to 100Hz
# PD control variables
P = 200
D = 20
# Simulator Loop
while not rospy.is_shutdown():
# reset simulator if trigger pressed
if buttons[0] == 1:
mvi.initialize_from_configs(0.0, q0, dt, q0)
# # calculate thrust inputs
# u1 = u0[0] + P*(q[4]+0.1*axis[0]) + D*system.configs[4].dq - P*(q[0]-3.0) - D*system.configs[0].dq
# u2 = u0[1] - P*(q[4]+0.1*axis[0]) - D*system.configs[4].dq - P*(q[0]-3.0) - D*system.configs[0].dq
# u3 = u0[2] - P*(q[3]+0.1*axis[1]) - D*system.configs[3].dq - P*(q[0]-3.0) - D*system.configs[0].dq
# u4 = u0[3] + P*(q[3]+0.1*axis[1]) + D*system.configs[3].dq - P*(q[0]-3.0) - D*system.configs[0].dq
# u = tuple([u1,u2,u3,u4])
k = np.array([0.1 * axis[1], 0.1 * axis[0]])
# advance simluation one timestep using trep VI
mvi.step(mvi.t2 + dt, u, k)
q = mvi.q2
t = mvi.t2
configs.data = tuple(q) + u
jointstates.header.stamp = rospy.Time.now()
jointstates.name = ["q1ballrx", "q1ballry"]
jointstates.position = [q[3], q[4]]
jointstates.velocity = [system.configs[5].dq, system.configs[6].dq]
# send transform data to rviz
broadcaster.sendTransform(
(q[2], q[1], q[0]),
tf.transformations.quaternion_from_euler(0, 0, 0),
rospy.Time.now(), "quad1", "world")
broadcaster.sendTransform(
(0, 0, 0), tf.transformations.quaternion_from_euler(q[5], q[6], 0),
rospy.Time.now(), "quadr", "quad1")
statepub.publish(jointstates)
pub.publish(configs)
r.sleep()
def create_initial_trajectory():
puppet = puppets.Puppet(joint_forces=False,
string_forces=False,
string_constraints=True)
puppet.q = {
"torso_rx": -0.05,
#"torso_ry" : 0.1,
"torso_tz": 0.0,
"lelbow_rx": 1.57,
"relbow_rx": 1.57,
"lhip_rx": pi / 2 - 0.6,
"rhip_rx": pi / 2 - 0.6,
"lknee_rx": -pi / 2 + 0.6,
"rknee_rx": -pi / 2 + 0.6,
}
puppet.project_string_controls()
q0 = puppet.q
u0 = tuple([0.0] * len(puppet.inputs))
qk2_0 = puppet.qk
dt = 0.01
# Create and initialize a variational integrator for the system.
gmvi = trep.MidpointVI(puppet)
gmvi.initialize_from_configs(0.0, q0, dt, q0)
left_leg_index = puppet.get_config("left_leg_string-length").k_index
right_leg_index = puppet.get_config("right_leg_string-length").k_index
left_arm_index = puppet.get_config("left_arm_string-length").k_index
right_arm_index = puppet.get_config("right_arm_string-length").k_index
def calc_vk():
v2 = [(q2 - q1) / (gmvi.t2 - gmvi.t1)
for (q2, q1) in zip(gmvi.q2, gmvi.q1)]
v2 = v2[len(puppet.dyn_configs):]
return v2
q = [gmvi.q2]
p = [gmvi.p2]
v = [calc_vk()]
t = [gmvi.t2]
while gmvi.t1 < 10.0:
# For our kinematic inputs, we first copy our starting position.
qk2 = list(qk2_0)
# Now we want to move some of the strings up and down.
qk2[left_leg_index] += -0.1 * sin(0.6 * pi * gmvi.t1)
qk2[right_leg_index] += 0.1 * sin(0.6 * pi * gmvi.t1)
qk2[left_arm_index] += 0.1 * sin(0.6 * pi * gmvi.t1)
qk2[right_arm_index] += -0.1 * sin(0.6 * pi * gmvi.t1)
qk2 = tuple(qk2)
gmvi.step(gmvi.t2 + dt, u0, qk2)
q.append(gmvi.q2)
p.append(gmvi.p2)
v.append(calc_vk())
t.append(gmvi.t2)
# The puppet can take a while to simulate, so print out the time
# occasionally to indicate our progress.
if abs(gmvi.t2 - round(gmvi.t2)) < dt / 2.0:
print "t =", gmvi.t2
dsys = discopt.DSystem(gmvi, t)
q = np.array(q)
rho = q[1:, puppet.nQd:]
X, U = dsys.build_trajectory(q, p, v, None, rho)
return puppet, dsys, t, X, U
def __init__(self, sys):
rospy.loginfo("Starting puppet simulator node")
self.legs_bool = rospy.get_param('legs', False)
self.shoulders_bool = rospy.get_param('shoulders', False)
# first, let's just define the initial configuration of the system:
self.sys = sys
self.q0_guess = {
# torso position
'TorsoZ' : BODY_HEIGHT,
# body robot
'BodyRobotTheta' : -pi/2,
'BodyRobotZ' : BODY_HEIGHT + SHOULDER_LENGTH,
'LeftShoulderString' : SHOULDER_LENGTH,
'RightShoulderString' : SHOULDER_LENGTH,
# left robot
'LeftRobotTheta' : -pi/2,
'LeftRobotX' : pd.torso_width/2,
'LeftRobotY' : -(pd.humerus_length + pd.radius_length),
'LeftRobotZ' : BODY_HEIGHT + ARM_LENGTH,
'LeftArmString' : ARM_LENGTH,
# right robot
'RightRobotTheta' : -pi/2,
'RightRobotX' : -pd.torso_width/2,
'RightRobotY' : -(pd.humerus_length + pd.radius_length),
'RightRobotZ' : BODY_HEIGHT + ARM_LENGTH,
'RightArmString' : ARM_LENGTH,
# left arm
'LShoulderPhi' : -pi/2,
# right arm
'RShoulderPhi' : -pi/2,
# shoulder robots
'LeftShoulderRobotTheta' : -pi/2,
'LeftShoulderRobotZ' : BODY_HEIGHT + SHOULDER_LENGTH,
'LeftShoulderRobotX' : pd.torso_width_1/2,
'RightShoulderRobotTheta' : -pi/2,
'RightShoulderRobotZ' : BODY_HEIGHT + SHOULDER_LENGTH,
'RightShoulderRobotX' : -pd.torso_width_1/2,
}
if self.legs_bool:
self.q0_guess.update( {
# left leg robot
'LeftLegRobotTheta' : -pi/2,
'LeftLegRobotX' : pd.torso_width/4,
'LeftLegRobotY' : -(pd.humerus_length + pd.radius_length),
'LeftLegRobotZ' : BODY_HEIGHT + LEG_LENGTH,
'LeftLegString' : LEG_LENGTH,
# right leg robot
'RightLegRobotTheta' : -pi/2,
'RightLegRobotX' : -pd.torso_width/4,
'RightLegRobotY' : -(pd.humerus_length + pd.radius_length),
'RightLegRobotZ' : BODY_HEIGHT + LEG_LENGTH,
'RightLegString' : LEG_LENGTH,
})
self.sys.q = 0
self.sys.q = self.q0_guess
self.sys.satisfy_constraints()
self.sys.minimize_potential_energy(keep_kinematic=True)
self.q0 = self.sys.q
self.mvi = trep.MidpointVI(self.sys)
self.mvi.initialize_from_configs(0, self.sys.q, DT, self.sys.q)
# fill out a message, and store it in the class so that I can update it
# whenever necessary
self.mappings = {
'left_shoulder_psi' : 'LShoulderPsi',
'left_shoulder_theta' : 'LShoulderTheta',
'left_shoulder_phi' : 'LShoulderPhi',
'right_shoulder_psi' : 'RShoulderPsi',
'right_shoulder_theta' : 'RShoulderTheta',
'right_shoulder_phi' : 'RShoulderPhi',
'left_elbow_phi' : 'LElbowPhi',
'right_elbow_phi' : 'RElbowPhi',
'left_hip_psi' : 'LHipPsi',
'left_hip_theta' : 'LHipTheta',
'left_hip_phi' : 'LHipPhi',
'right_hip_psi' : 'RHipPsi',
'right_hip_theta' : 'RHipTheta',
'right_hip_phi' : 'RHipPhi',
'left_knee_phi' : 'LKneePhi',
'right_knee_phi' : 'RKneePhi',
}
self.js = JS()
self.names = [x for x in self.mappings.keys()]
self.js.name = self.names
self.js.header.frame_id = 'world'
self.update_values()
self.count_exceptions = 0
# define tf broadcaster and listener
self.br = tf.TransformBroadcaster()
self.listener = tf.TransformListener()
# add constraint visualization:
self.con_vis = []
# hands:
for i in 4,5:
self.con_vis.append(StringMarker(fm[i].input_frame,
fm[i].puppet_frames,
self.listener))
if self.shoulders_bool: # shoulders
for i in 2,3:
self.con_vis.append(StringMarker(fm[i].input_frame,
fm[i].puppet_frames,
self.listener))
else: # body
for i in 0,1:
self.con_vis.append(StringMarker(fm[1].input_frame,
fm[1].puppet_frames[i],
self.listener))
if self.legs_bool: # legs
for i in 6,7:
self.con_vis.append(StringMarker(fm[i].input_frame,
fm[i].puppet_frames,
self.listener))
# define a publisher for the joint states
self.joint_pub = rospy.Publisher("joint_states", JS)
# define a publisher for the constraint Markers
self.con_pub = rospy.Publisher("visualization_markers", VM.MarkerArray)
# define a timer for publishing the frames and tf's
rospy.Timer(rospy.Duration(1.0/tf_freq), self.send_joint_states)
# request that we get a service handler:
rospy.loginfo("Waiting for reset handling service...")
rospy.wait_for_service("interface_reset")
self.reset_srv_client = rospy.ServiceProxy("interface_reset", SS.Empty)
# offer service to reset simulation
self.reset_srv_provider = rospy.Service("simulator_reset", SS.Empty,
self.reset_provider)
# define a timer for integrating the state of the VI
rospy.loginfo("Starting integration...")
rospy.Timer(rospy.Duration(DT), self.integrate_vi)
if __name__ == '__main__':
puppet = Puppet(joint_forces=False,
string_forces=False,
string_constraints=True)
puppet.get_config('torso_rx').q = 0.1
puppet.get_config('torso_ry').q = 0.1
puppet.project_string_controls()
q0 = puppet.get_q()
u0 = tuple([0.0] * len(puppet.inputs))
qk2 = puppet.get_qk()
dt = 0.01
# Create and initialize a variational integrator for the system.
gmvi = trep.MidpointVI(puppet)
gmvi.initialize_from_configs(0.0, q0, dt, q0)
q = [gmvi.q2]
t = [gmvi.t2]
while gmvi.t1 < 10.0:
gmvi.step(gmvi.t2 + dt, u0, qk2)
q.append(gmvi.q2)
t.append(gmvi.t2)
# The puppet can take a while to simulate, so print out the time
# occasionally to indicate our progress.
if abs(gmvi.t2 - round(gmvi.t2)) < dt / 2.0:
print "t =", gmvi.t2
visualize_3d([PuppetVisual(puppet, t, q)])
tf = 10.0
dt = 0.01
# Define a simple 2 link pendulum with a force input on the middle joint
system = trep.System()
frames = [
tz(-1), # Provided as an angle reference
ry("theta1"),
[tz(-2, mass=1), [ry("theta2"), [tz(-2, mass=1)]]]
]
system.import_frames(frames)
trep.potentials.Gravity(system, (0, 0, -9.8))
trep.forces.Damping(system, 1.2)
trep.forces.ConfigForce(system, 'theta2', 'theta2-torque')
mvi = trep.MidpointVI(system) # Create the variational integrator
# Define a function to run the simulation and return the result
def simulate(mvi, u_func, dt, tf):
q = [mvi.q2]
p = [mvi.p2]
t = [mvi.t2]
while mvi.t1 < tf:
mvi.step(mvi.t2 + dt, u_func(mvi.t1))
q.append(mvi.q2)
p.append(mvi.p2)
t.append(mvi.t2)
return t, q, p
amp = np.radians(10) # amplitude in radians
freq = 8 # frequency in Hz.
def h_func(t):
return amp*np.sin(2*np.pi*freq*t)
def hdt_func(t):
return 2*np.pi*freq*amp*np.cos(2*np.pi*freq*t)
base_motion = {'h': h_func, 'hdt': hdt_func}
# Create the whisker and the integrator.
fx = []
for a in [0, 10, 20, 30]:
parameters['a'] = a
whisker = ws.whisker.Whisker2D(parameters, base_motion)
mvi = trep.MidpointVI(whisker)
# Simulate and visualize the whisking.
tf = 2.0
dt = 0.005
(t, q, lam) = ws.collisions.sim.simulate(mvi, tf, dt)
base_forces = ws.simulation.simulate.get_base_forces_2d(whisker, lam, dt)
fx.append(base_forces['Fy']*1e6)
# Plot the results for each curvature.
N = 200
plt.rc('text', usetex=True)
matplotlib.rcParams.update({'font.size':18})
fig = plt.figure(facecolor='white',figsize=(12,5))
ax = plt.axes()
# There is no guarantee that the configuration we just set is
# consistent with the system's constraints. We can call
# System.satisfy_constraints() to have trep find a configuration that
# is consistent. This just uses a root-finding method to solve h(q) =
# 0 starting from the current configuration. There is no guarantee
# that it will always converge and no guarantee the final
# configuration is close to the original one.
system.satisfy_constraints()
# Now we'll extract the current configuration into a tuple to use as
# initial conditions for a variational integrator.
q0 = system.q
# Create and initialize the variational integrator
mvi = trep.MidpointVI(system)
mvi.initialize_from_configs(0.0, q0, dt, q0)
# This is our simulation loop. We save the results in two lists.
q = [mvi.q2]
t = [mvi.t2]
while mvi.t1 < tf:
mvi.step(mvi.t2 + dt)
q.append(mvi.q2)
t.append(mvi.t2)
# After the simulation is finished, we can visualize the results. The
# viewer can automatically draw a primitive representation for
# arbitrary systems from the tree structure. print_instructions() has
# the viewer print out basic usage information to the console.
visual.visualize_3d([visual.VisualItem3D(system, t, q)])
def QuadMain(isjoy):
def make_state_cost(dsys, base, x):
weight = base * np.ones((dsys.nX, ))
weight[system.get_config('ballx').index] = 50
weight[system.get_config('bally').index] = 50
weight[system.get_config('ballz').index] = 25
weight[system.get_config('quad1bry').index] = 500
weight[system.get_config('quad1brx').index] = 100
weight[system.get_config('quad1ry').index] = 10
weight[system.get_config('quad1rx').index] = 10
weight[system.get_config('quad1rz').index] = 1
weight[system.get_config('quad2bry').index] = 500
weight[system.get_config('quad2brx').index] = 15
weight[system.get_config('quad2ry').index] = 10
weight[system.get_config('quad2rx').index] = 10
weight[system.get_config('quad2rz').index] = 1
return np.diag(weight)
def make_input_cost(dsys, base, x):
weight = base * np.ones((dsys.nU, ))
# weight[system.get_input('x-force').index] = x
return np.diag(weight)
quad1cm_m = 1.0
quadmotor1_m = 1.0
quad2cm_m = 1.0
quadmotor2_m = 1.0
ball_m = 0.5
dt = 0.01 # timestep set to 10ms
# Create a new trep system - 6 generalized coordinates for quadrotor: x,y,z,roll,pitch,yaw
system = trep.System()
trep.potentials.Gravity(system, name="Gravity")
# define ball frame
ballz = trep.Frame(system.world_frame, trep.TZ, "ballz")
bally = trep.Frame(ballz, trep.TY, "bally")
ballx = trep.Frame(bally, trep.TX, "ballx")
ballx.set_mass(ball_m)
# define quadrotor 1 frame
quad1bry = trep.Frame(ballx, trep.RY, "quad1bry")
quad1brx = trep.Frame(quad1bry, trep.RX, "quad1brx")
quad1cm = trep.Frame(quad1brx, trep.CONST_SE3,
((1, 0, 0), (0, 1, 0), (0, 0, 1), (0, 0, 2)),
"quad1cm")
quad1ry = trep.Frame(quad1cm, trep.RY, "quad1ry")
quad1rx = trep.Frame(quad1ry, trep.RX, "quad1rx")
quad1rz = trep.Frame(quad1rx, trep.RZ, "quad1rz")
quad1cm.set_mass(quad1cm_m) # central point mass
# define quadrotor 1 motor positions and mass
quad1m1 = trep.Frame(quad1rz, trep.CONST_SE3,
((1, 0, 0), (0, 1, 0), (0, 0, 1), (1, 0, 0)),
"quad1m1")
quad1m1.set_mass(quadmotor1_m)
quad1m2 = trep.Frame(quad1rz, trep.CONST_SE3,
((1, 0, 0), (0, 1, 0), (0, 0, 1), (-1, 0, 0)),
"quad1m2")
quad1m2.set_mass(quadmotor1_m)
quad1m3 = trep.Frame(quad1rz, trep.CONST_SE3,
((1, 0, 0), (0, 1, 0), (0, 0, 1), (0, 1, 0)),
"quad1m3")
quad1m3.set_mass(quadmotor1_m)
quad1m4 = trep.Frame(quad1rz, trep.CONST_SE3,
((1, 0, 0), (0, 1, 0), (0, 0, 1), (0, -1, 0)),
"quad1m4")
quad1m4.set_mass(quadmotor1_m)
# set four thrust vectors with inputs u1,u2,u3,u4
trep.forces.BodyWrench(system,
quad1m1, (0, 0, 'u1q1', 0, 0, 0),
name='quad1w1')
trep.forces.BodyWrench(system,
quad1m2, (0, 0, 'u2q1', 0, 0, 0),
name='quad1w2')
trep.forces.BodyWrench(system,
quad1m3, (0, 0, 'u3q1', 0, 0, 0),
name='quad1w3')
trep.forces.BodyWrench(system,
quad1m4, (0, 0, 'u4q1', 0, 0, 0),
name='quad1w4')
# define quadrotor 2 frame
quad2bry = trep.Frame(ballx, trep.RY, "quad2bry")
quad2brx = trep.Frame(quad2bry, trep.RX, "quad2brx")
quad2cm = trep.Frame(quad2brx, trep.CONST_SE3,
((1, 0, 0), (0, 1, 0), (0, 0, 1), (0, 0, 2)),
"quad2cm")
quad2ry = trep.Frame(quad2cm, trep.RY, "quad2ry")
quad2rx = trep.Frame(quad2ry, trep.RX, "quad2rx")
quad2rz = trep.Frame(quad2rx, trep.RZ, "quad2rz")
quad2cm.set_mass(quad2cm_m) # central point mass
# define quadrotor 2 motor positions and mass
quad2m1 = trep.Frame(quad2rz, trep.CONST_SE3,
((1, 0, 0), (0, 1, 0), (0, 0, 1), (1, 0, 0)),
"quad2m1")
quad2m1.set_mass(quadmotor2_m)
quad2m2 = trep.Frame(quad2rz, trep.CONST_SE3,
((1, 0, 0), (0, 1, 0), (0, 0, 1), (-1, 0, 0)),
"quad2m2")
quad2m2.set_mass(quadmotor2_m)
quad2m3 = trep.Frame(quad2rz, trep.CONST_SE3,
((1, 0, 0), (0, 1, 0), (0, 0, 1), (0, 1, 0)),
"quad2m3")
quad2m3.set_mass(quadmotor2_m)
quad2m4 = trep.Frame(quad2rz, trep.CONST_SE3,
((1, 0, 0), (0, 1, 0), (0, 0, 1), (0, -1, 0)),
"quad2m4")
quad2m4.set_mass(quadmotor2_m)
# set four thrust vectors with inputs u1,u2,u3,u4
trep.forces.BodyWrench(system,
quad2m1, (0, 0, 'u1q2', 0, 0, 0),
name='quad2w1')
trep.forces.BodyWrench(system,
quad2m2, (0, 0, 'u2q2', 0, 0, 0),
name='quad2w2')
trep.forces.BodyWrench(system,
quad2m3, (0, 0, 'u3q2', 0, 0, 0),
name='quad2w3')
trep.forces.BodyWrench(system,
quad2m4, (0, 0, 'u4q2', 0, 0, 0),
name='quad2w4')
# set quadrotor initial altitude at 5m
system.get_config("ballz").q = 0.0
system.get_config("quad1bry").q = (math.pi / 2 - math.acos(1.5 / 2.0))
system.get_config("quad2bry").q = -(math.pi / 2 - math.acos(1.5 / 2.0))
horzf = 0.5 * ball_m * 9.8 * math.tan((math.pi / 2 - math.acos(1.5 / 2.0)))
vertf = (0.5 * ball_m + quad1cm_m + 4 * quadmotor1_m) * 9.8
quad1ang = math.atan(horzf / ((quad1cm_m + 4 * quadmotor1_m) * 9.8))
system.get_config(
"quad1ry").q = -(math.pi / 2 - math.acos(1.5 / 2.0)) + quad1ang
system.get_config("quad2ry").q = (math.pi / 2 -
math.acos(1.5 / 2.0)) - quad1ang
# Now we'll extract the current configuration into a tuple to use as
# initial conditions for a variational integrator.
q0 = system.q
# Create the discrete system
mvi = trep.MidpointVI(system)
t = np.arange(0.0, 10.0, 0.01)
dsys = discopt.DSystem(mvi, t)
# Generate cost function
Qcost = make_state_cost(dsys, 1, 0.00)
Rcost = make_input_cost(dsys, 0.01, 0.01)
totuf = math.sqrt(math.pow(horzf, 2) + math.pow(vertf, 2))
u0 = np.array([
totuf / 4, totuf / 4, totuf / 4, totuf / 4, totuf / 4, totuf / 4,
totuf / 4, totuf / 4
])
u = tuple(u0)
mvi.initialize_from_configs(0.0, q0, dt, q0)
pref = mvi.p2
Uint = np.zeros((len(t) - 1, system.nu))
qd = np.zeros((len(t), system.nQ))
pd = np.zeros((len(t), system.nQ))
for i, t in enumerate(t[:-1]):
Uint[i, :] = u0
qd[i, :] = q0
pd[i, :] = pref
qd[len(qd) - 1, :] = q0
pd[len(pd) - 1, :] = pref
Qk = lambda k: Qcost
(X, U) = dsys.build_trajectory(Q=qd, p=pd, u=Uint)
Kstab = dsys.calc_feedback_controller(X, U, Qk)
#print Kstab[0]
# Create and initialize the variational integrator)
u = tuple(u0)
#mvi.initialize_from_configs(0.0, q0, dt, q0)
# These are our simulation variables.
q = mvi.q2
p = mvi.p2
pref = p
t = mvi.t2
# start ROS node to broadcast transforms to rviz
rospy.init_node('quad_simulator')
broadcaster = tf.TransformBroadcaster()
pub = rospy.Publisher('config', Float32MultiArray, queue_size=2)
statepub = rospy.Publisher('joint_states', JointState, queue_size=2)
jointstates = JointState()
configs = Float32MultiArray()
if isjoy == True:
markerpub = rospy.Publisher('refmark', Marker, queue_size=2)
refmark = Marker()
if isjoy == False:
############### NEW ################################33
# create an interactive marker server on the topic namespace simple_marker
server = InteractiveMarkerServer("simple_marker")
# create an interactive marker for our server
int_marker = InteractiveMarker()
int_marker.header.frame_id = "/world"
int_marker.name = "my_marker"
int_marker.description = "Simple 1-DOF Control"
int_marker.pose.position.z = 2.0
# create a grey box marker
box_marker = Marker()
box_marker.type = Marker.SPHERE
box_marker.scale.x = 0.15
box_marker.scale.y = 0.15
box_marker.scale.z = 0.15
box_marker.color.r = 0.0
box_marker.color.g = 1.0
box_marker.color.b = 0.0
box_marker.color.a = 1.0
# create a non-interactive control which contains the box
box_control = InteractiveMarkerControl()
box_control.orientation.w = 1
box_control.orientation.x = 0
box_control.orientation.y = 1
box_control.orientation.z = 0
box_control.name = "rotate_x"
box_control.name = "move_x"
box_control.always_visible = True
box_control.interaction_mode = InteractiveMarkerControl.MOVE_PLANE
int_marker.controls.append(box_control)
box_control = InteractiveMarkerControl()
box_control.orientation.w = 1
box_control.orientation.x = 0
box_control.orientation.y = 1
box_control.orientation.z = 0
box_control.name = "rotate_x"
box_control.name = "move_x"
box_control.always_visible = True
box_control.interaction_mode = InteractiveMarkerControl.MOVE_PLANE
box_control.markers.append(box_marker)
int_marker.controls.append(box_control)
# add the interactive marker to our collection &
# tell the server to call processFeedback() when feedback arrives for it
server.insert(int_marker, processFeedback)
# 'commit' changes and send to all clients
server.applyChanges()
############### END NEW ###############################
if isjoy == True:
# subscribe to joystick topic from joy_node
rospy.Subscriber("joy", Joy, joycall)
r = rospy.Rate(100) # simulation rate set to 100Hz
# Simulator Loop
while not rospy.is_shutdown():
# reset simulator if trigger pressed
if buttons[0] == 1:
mvi.initialize_from_configs(0.0, q0, dt, q0)
qref = [0, axis[1], axis[0], 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
u = tuple(u0 - np.dot(Kstab[0], np.append(q - q0 - qref, p - pref)))
# advance simluation one timestep using trep VI
try:
mvi.step(mvi.t2 + dt, u)
except trep.ConvergenceError:
print 'Trep simulation error - system resetting...'
rospy.sleep(2.)
mvi.initialize_from_configs(0.0, q0, dt, q0)
q = mvi.q2
p = mvi.p2
t = mvi.t2
configs.data = tuple(q) + u
if isjoy == True:
refmark.header.frame_id = "/world"
refmark.header.stamp = rospy.Time.now()
refmark.type = 2
refmark.scale.x = 0.15
refmark.scale.y = 0.15
refmark.scale.z = 0.15
refmark.color.r = 0.0
refmark.color.g = 1.0
refmark.color.b = 0.0
refmark.color.a = 1.0
refmark.pose.position.y = axis[1]
refmark.pose.position.x = axis[0]
refmark.pose.position.z = 2.0
jointstates.header.stamp = rospy.Time.now()
jointstates.name = [
"quad1bry", "quad1brx", "quad1ry", "quad1rx", "quad1rz",
"quad2bry", "quad2brx", "quad2ry", "quad2rx", "quad2rz"
]
jointstates.position = [
q[3], q[4], q[5], q[6], q[7], q[8], q[9], q[10], q[11], q[12]
]
jointstates.velocity = [
system.configs[3].dq, system.configs[4].dq, system.configs[5].dq,
system.configs[6].dq, system.configs[7].dq, system.configs[8].dq,
system.configs[9].dq, system.configs[10].dq, system.configs[11].dq,
system.configs[12].dq
]
# send transform data to rviz
broadcaster.sendTransform(
(q[2], q[1], q[0] + 2),
tf.transformations.quaternion_from_euler(0.0, 0.0, 0.0),
rospy.Time.now(), "ball", "world")
statepub.publish(jointstates)
pub.publish(configs)
if isjoy == True:
markerpub.publish(refmark)
r.sleep()