def build_system(torque_force=False):
cart_mass = 1
pendulum_length = 1.0
pendulum_mass = 0.1
system = trep.System()
frames = [
rz('theta', name="pendulumShoulder"),
[
tx(-pendulum_length, name="pendulumArm1", mass=pendulum_mass),
[
rz('x', name="pendulumElbow"),
[
tx(-pendulum_length,
name="pendulumArm2",
mass=pendulum_mass)
]
]
]
]
system.import_frames(frames)
## Because 2D system
trep.potentials.Gravity(system, (0, -9.8, 0))
trep.forces.Damping(system, 10)
trep.forces.ConfigForce(system, 'x', 'x-force')
if torque_force:
trep.forces.ConfigForce(system, 'theta', 'theta-force')
return system
def falling_links():
""" Create a scissor lift according to the global simulation parameters. """
def add_level(left, right, link=0):
"""
Recursive function to build the scissor lift by attaching
another set of links. 'left' and 'right' are the coordinate
frames to attach the left and right links to.
"""
if link == segments: # Terminate the recusions
return (left, right)
# Create the base of the left link
left = Frame(left, trep.RY, 'Theta_Left')
# "L%02d" % link
left.config.q = th_l1
# Create a frame at the middle of the link to attach the link's mass.
left_mid = Frame(left, trep.TX, L_link / 2.0)
left_mid.set_mass(m_link, I_link, I_link, I_link)
# Add the end of the link.
left_end = Frame(left, trep.TX, L_link)
right = Frame(right, trep.RY, 'Theta_Right')
right.config.q = th_l2
right_mid = Frame(right, trep.TX, L_link / 2.0)
right_mid.set_mass(m_link, I_link, I_link, I_link)
right_end = Frame(right, trep.TX, L_link)
# Join the two links at the middle.
#trep.constraints.PointToPoint2D(system, 'xz', left_mid, right_mid)
# Add a new level. Note that left and right switch each time
return add_level(right_end, left_end, link + 1)
# Create the new system
system = trep.System()
trep.potentials.Gravity(system, name="Gravity")
# Add free fall variable
drop = Frame(system.world_frame, trep.TZ, "DROP")
drop.config.q = 10. #L_link*math.cos(theta_0)
drop.set_mass(0.0)
wiggle = Frame(drop, trep.TY, "WIGGLE")
wiggle.config.q = 10. #L_link*math.cos(theta_0)
wiggle.set_mass(0.0)
# Add the top slider
slide = Frame(wiggle, trep.TX, "SLIDE")
slide.config.q = 0. #L_link*math.cos(theta_0)
slide.set_mass(0.0)
# Create all the links in the system.
add_level(slide, slide)
# The scissor lift should satisfy the constraints, but call
# satisfy_constraints() in case it needs to be nudged a little
# from numerical error.
trep.forces.Damping(system, 0.0, {
'DROP': dampz,
'WIGGLE': dampy,
'SLIDE': dampx
})
system.satisfy_constraints()
return system
def make_scissor_lift():
""" Create a scissor lift according to the global simulation parameters. """
def add_level(left, right, link=0):
"""
Recursive function to build the scissor lift by attaching
another set of links. 'left' and 'right' are the coordinate
frames to attach the left and right links to.
"""
if link == segments: # Terminate the recusions
return (left, right)
# Create the base of the left link
left = Frame(left, trep.RY, "L%02d" % link, "L%02d" % link)
if link == 0:
left.config.q = theta_0
else:
left.config.q = mpi + 2.0 * theta_0
# Create a frame at the middle of the link to attach the link's mass.
left_mid = Frame(left, trep.TX, L_link / 2.0)
left_mid.set_mass(m_link, I_link, I_link, I_link)
# Add the end of the link.
left_end = Frame(left, trep.TX, L_link)
right = Frame(right, trep.RY, "R%02d" % link, "R%02d" % link)
if link == 0:
right.config.q = mpi - theta_0
else:
right.config.q = mpi - 2.0 * theta_0
right_mid = Frame(right, trep.TX, L_link / 2.0)
right_mid.set_mass(m_link, I_link, I_link, I_link)
right_end = Frame(right, trep.TX, L_link)
# Join the two links at the middle.
trep.constraints.PointToPoint2D(system, 'xz', left_mid, right_mid)
# Add a new level. Note that left and right switch each time
return add_level(right_end, left_end, link + 1)
# Create the new system
system = trep.System()
trep.potentials.Gravity(system, name="Gravity")
# Add the top slider
slider = Frame(system.world_frame, trep.TX, "SLIDER")
slider.config.q = L_link * math.cos(theta_0)
slider.set_mass(m_slider)
# Create all the links in the system.
add_level(system.world_frame, slider)
# The scissor lift should satisfy the constraints, but call
# satisfy_constraints() in case it needs to be nudged a little
# from numerical error.
system.satisfy_constraints()
return system
def make_pendulum(num_links, hybrid_wrenched=False):
"""
make_pendulum(num_links, hybrid_wrenched) -> System
Create a forced pendulum system with num_links. The pendulum is
actuated by either direct joint torques or body wrenches in the
local X direction depending on the hybrid_wrenched parameter.
"""
def add_level(frame, link=0):
"""
Recusively add links to a system by attaching a new link to
the specified frame.
"""
if link == num_links:
return
# Create a rotation for the pendulum.
# The first argument is the name of the frame, the second is
# the transformation type, and the third is the name of the
# configuration variable that parameterizes the
# transformation.
config_name = 'theta-%d' % link
child = trep.Frame(frame, trep.RY, config_name, "link-%d" % link)
if not hybrid_wrenched:
trep.forces.ConfigForce(child.system,
config_name,
'torque-%d' % link,
name='joint-force-%d' % link)
# Move down to create the length of the pendulum link.
child = trep.Frame(child, trep.TZ, -1)
# Add mass to the end of the link (only a point mass, no
# rotational inertia)
child.set_mass(1.0)
if hybrid_wrenched:
trep.forces.HybridWrench(child.system,
child, ('wrench-x-%d' % link, 0.0, 0.0),
name='wrench-%d' % link)
add_level(child, link + 1)
# Create a new system, add the pendulum links, and rotate the top
# pendulum.
system = trep.System()
trep.potentials.Gravity(system, name="Gravity")
trep.forces.Damping(system, 1.0)
add_level(system.world_frame)
return system
def MassSystem2D():
# define system:
system = trep.System()
frames = [
tx('xm', name='x-mass'), [ty('ym', name='y-mass', mass=BALL_MASS)],
ty(1, name='robot_plane'), [tx('xr', name='x-robot', kinematic=True)]
]
system.import_frames(frames)
trep.potentials.Gravity(system, (0, -g, 0))
trep.forces.Damping(system, 0.05)
# add string constraint as a kinematic configuration var
trep.constraints.Distance(system, 'y-mass', 'x-robot', 'r')
return system
def build_system(torque_force=False):
cart_mass = 10.0
pendulum_length = 1.0
pendulum_mass = 1.0
system = trep.System()
frames = [
tx('x', name='Cart', mass=cart_mass), [
rz('theta', name="PendulumBase"), [
ty(-pendulum_length, name="Pendulum", mass=pendulum_mass)]]]
system.import_frames(frames)
trep.potentials.Gravity(system, (0, -9.8, 0))
trep.forces.Damping(system, 0.01)
trep.forces.ConfigForce(system, 'x', 'x-force')
if torque_force:
trep.forces.ConfigForce(system, 'theta', 'theta-force')
return system
def make_pendulum(num_links, kinematic=False):
"""
make_pendulum(num_links) -> System
Create a pendulum system with num_links.
"""
def add_level(frame, link=0, kinematic=False):
"""
Recursively add links to a system by attaching a new link to
the specified frame.
"""
if link == num_links:
return
# Create a rotation for the pendulum.
# The first argument is the name of the frame, the second is
# the transformation type, and the third is the name of the
# configuration variable that parameterizes the
# transformation.
child = trep.Frame(frame,
trep.RX,
"link-%d" % link,
"link-%d" % link,
kinematic=kinematic)
# Move down to create the length of the pendulum link.
child = trep.Frame(child, trep.TZ, -1)
# Add mass to the end of the link (only a point mass, no
# rotational intertia
child.set_mass(1.0)
add_level(child, link + 1)
# Create a new system, add the pendulum links, and rotate the top
# pendulum.
system = trep.System()
trep.potentials.Gravity(system, name="Gravity")
add_level(system.world_frame, kinematic=kinematic)
system.get_config("link-0").q = math.pi / 2.
return system
# First define the open-chain coordinate frames of the system. Here
# we have named each frame to correspond to a paper figure (Johnson
# and Murphey). In general, however, we only need to name frames
# that we reference later in constraints.
import sys
import trep
from trep import tx, ty, tz, rx, ry, rz
import trep.visual as visual
tf = 10.0
dt = 0.01
# Define the mechanical system
system = trep.System()
frames = [
rx('J', name='J'),
[
tz(-0.5, name='I', mass=1),
tz(-1),
[rx('H', name='H'), [tz(-1, name='G', mass=1),
tz(-2, name='O2')]]
],
ty(1.5),
[
rx('K', name='K'),
[
tz(-1, name='L', mass=1),
tz(-2),
[
def falling_links():
# Create the new system
system = trep.System()
trep.potentials.Gravity(system, name="Gravity")
# Add free fall variable
fallframe = Frame(system.world_frame, trep.TZ, "DROP")
fallframe.config.q = 0. #L_link*math.cos(theta_0)
fallframe = Frame(fallframe, trep.TY, "WIGGLE")
fallframe = Frame(fallframe, trep.TX, "SLIDE")
fallframe.set_mass(0.)
# Create all the links in the system.
#add_level(fallframe, fallframe)
# Create the base of the left link
left = Frame(fallframe, trep.RX, 'ROLL')
left = Frame(left, trep.RY, 'PITCH')
left_mid = Frame(left, trep.RZ, 'YAW')
left_mid.set_mass(m_link, I_link, I_link, I_link)
lcf = 1. / 8.
# "L%02d" % link
# Create a frame at the middle of the link to attach the link's mass.
left_leg1 = Frame(left_mid, trep.RZ, mpi / 4.)
left_leg1 = Frame(left_mid, trep.RZ, mpi / 4.)
left_leg1 = Frame(left_leg1, trep.TX, -L_link / 2.0)
left_leg1.set_mass(lcf * m_link, lcf * I_link, lcf * I_link, lcf * I_link)
# Add the end of the link.
left_leg2 = Frame(left_mid, trep.RZ, mpi / 2 + mpi / 4.)
left_leg2 = Frame(left_leg2, trep.TX, L_link / 2.0)
left_leg2.set_mass(lcf * m_link, lcf * I_link, lcf * I_link, lcf * I_link)
# Create a frame at the middle of the link to attach the link's mass.
left_leg3 = Frame(left_mid, trep.RZ, mpi + mpi / 4.)
left_leg3 = Frame(left_leg3, trep.TX, -L_link / 2.0)
left_leg3.set_mass(lcf * m_link, lcf * I_link, lcf * I_link, lcf * I_link)
# Add the end of the link.
left_leg4 = Frame(left_mid, trep.RZ, 3 * mpi / 2 + mpi / 4.)
left_leg4 = Frame(left_leg4, trep.TX, L_link / 2.0)
left_leg4.set_mass(lcf * m_link, lcf * I_link, lcf * I_link, lcf * I_link)
###################################################################
# # Passenger frame
right = Frame(left_mid, trep.RX, 'PAXx')
#right.config.q = -0.5*mpi #left.config.q - mpi*0.5
right = Frame(right, trep.RY, 'PAXy')
pcf = 100. / m_link
plcf = 2.074 / L_link
stomachat = 8.0
#right.config.q = -0.5*mpi #left.config.q - mpi*0.5
right_mid = Frame(right, trep.TZ, plcf * L_link / stomachat)
right_mid.set_mass(pcf * m_link, plcf * pcf * I_link, plcf * pcf * I_link,
plcf * pcf * I_link)
right_end = Frame(right_mid, trep.TZ,
(stomachat - 1) * plcf * L_link / stomachat)
# Join the two links at the middle.
# trep.constraints.PointToPoint2D(system, 'xz', left_mid, right_mid)
# 6.15
trep.forces.Damping(
system, 0.0, {
'DROP': 1.,
'WIGGLE': 0.5,
'SLIDE': 0.5,
'ROLL': 0.2,
'PITCH': 0.2,
'YAW': 0.2,
'PAXx': 100.,
'PAXy': 100.
})
#trep.forces.BodyWrench(system, fallframe, wrench=(0., 0., 'forcez', 'm1', 'm2', 'm3'))
# the passengers response motion
trep.forces.ConfigForce(system, 'DROP', 'fz') # Add input
trep.forces.ConfigForce(system, 'PITCH', 'm1') # Add input
trep.forces.ConfigForce(system, 'ROLL', 'm2') # Add input
trep.forces.ConfigForce(system, 'YAW', 'm3') # Add input
trep.forces.ConfigForce(system, 'PAXy', 'fpy') # Add input
trep.forces.ConfigForce(system, 'PAXx', 'fpx') # Add input
system.satisfy_constraints()
return system
# Import necessary Python modules
import math
from math import pi
import numpy as np
import trep
from trep import tx, ty, tz, rx, ry, rz
# Build a pendulum system
m = 1.0 # Mass of pendulum
l = 1.0 # Length of pendulum
q0 = 3. / 4. * pi # Initial configuration of pendulum
t0 = 0.0 # Initial time
tf = 5.0 # Final time
dt = 0.1 # Timestep
system = trep.System() # Initialize system
frames = [
rx('theta', name="pendulumShoulder"), [tz(-l, name="pendulumArm", mass=m)]
]
system.import_frames(frames) # Add frames
# Add forces to the system
trep.potentials.Gravity(system, (0, 0, -9.8)) # Add gravity
trep.forces.Damping(system, 1.0) # Add damping
trep.forces.ConfigForce(system, 'theta', 'theta-torque') # Add input
# Create and initialize the variational integrator
mvi = trep.MidpointVI(system)
mvi.initialize_from_configs(t0, np.array([q0]), t0 + dt, np.array([q0]))
def falling_links():
# Create the new system
system = trep.System()
trep.potentials.Gravity(system, name="Gravity")
# Add free fall variable
fallframe = Frame(system.world_frame, trep.TZ, "DROP")
#fallframe.config.q = 10. #L_link*math.cos(theta_0)
fallframe = Frame(fallframe, trep.TY, "WIGGLE")
fallframe = Frame(fallframe, trep.TX, "SLIDE")
fallframe.set_mass(0.0)
# Create all the links in the system.
#add_level(fallframe, fallframe)
# Create the base of the left link
left = Frame(fallframe, trep.RX, 'ROLL')
left = Frame(left, trep.RY, 'PITCH')
left_mid = Frame(left, trep.RZ, 'YAW')
left_mid.set_mass(m_link, I_link, I_link, I_link)
# "L%02d" % link
# Create a frame at the middle of the link to attach the link's mass.
left_leg1 = Frame(left_mid, trep.RZ, mpi / 4.)
left_leg1 = Frame(left_leg1, trep.TX, -L_link / 2.0)
left_leg1.set_mass(0.25 * m_link, 0.25 * I_link, 0.25 * I_link,
0.25 * I_link)
# Add the end of the link.
left_leg2 = Frame(left_mid, trep.RZ, mpi / 2 + mpi / 4.)
left_leg2 = Frame(left_leg2, trep.TX, L_link / 2.0)
left_leg2.set_mass(0.25 * m_link, 0.25 * I_link, 0.25 * I_link,
0.25 * I_link)
# Create a frame at the middle of the link to attach the link's mass.
left_leg3 = Frame(left_mid, trep.RZ, mpi + mpi / 4.)
left_leg3 = Frame(left_leg3, trep.TX, -L_link / 2.0)
left_leg3.set_mass(0.25 * m_link, 0.25 * I_link, 0.25 * I_link,
0.25 * I_link)
# Add the end of the link.
left_leg4 = Frame(left_mid, trep.RZ, 3 * mpi / 2 + mpi / 4.)
left_leg4 = Frame(left_leg4, trep.TX, L_link / 2.0)
left_leg4.set_mass(0.25 * m_link, 0.25 * I_link, 0.25 * I_link,
0.25 * I_link)
###################################################################
# # Passenger frame
# right = Frame(left_mid, trep.RY, 'PAX')
# right.config.q = -0.5*mpi + 0.05 #left.config.q - mpi*0.5
# right_mid = Frame(right, trep.TX, L_link/9.0)
# right_mid.set_mass(0.5*m_link, 0.5*I_link, 0.5*I_link, 0.5*I_link)
# right_end = Frame(right, trep.TX, L_link/3.0)
# Join the two links at the middle.
# trep.constraints.PointToPoint2D(system, 'xz', left_mid, right_mid)
trep.forces.Damping(system, 0.0, {
'DROP': 0.05,
'WIGGLE': 0.18,
'SLIDE': 0.18
})
trep.forces.BodyWrench(system,
fallframe,
wrench=(0.1, 0.1, 'forcez', 'moment1', 'moment2',
'moment3'))
# the passengers response motion
# trep.forces.ConfigForce(system, 'PAX', 'response-torque') # Add input
system.satisfy_constraints()
return system
def falling_links():
""" Create a scissor lift according to the global simulation parameters. """
def add_level(left, right, link=0):
"""
Recursive function to build the scissor lift by attaching
another set of links. 'left' and 'right' are the coordinate
frames to attach the left and right links to.
"""
if link == segments: # Terminate the recusions
return (left, right)
# Create the base of the left link
left = Frame(left, trep.RX, 'ROLL')
left = Frame(left, trep.RY, 'PITCH')
left_mid = Frame(left, trep.RZ, 'YAW')
left_mid.set_mass(m_link, I_link, I_link, I_link)
# "L%02d" % link
if link == 0:
left.config.q = 0.05 #theta_0
else:
left.config.q = theta_0
# Create a frame at the middle of the link to attach the link's mass.
left_begin = Frame(left_mid, trep.TX, -L_link / 2.0)
left_begin.set_mass(0.25 * m_link, 0, 0, 0)
# Add the end of the link.
left_end = Frame(left_mid, trep.TX, L_link / 2.0)
left_end.set_mass(0.25 * m_link, 0, 0, 0)
right = Frame(left_mid, trep.RY, 'PAX')
if link == 0:
right.config.q = -0.5 * mpi + 0.05 #left.config.q - mpi*0.5
else:
right.config.q = left.config.q - mpi * 0.5
right_mid = Frame(right, trep.TX, L_link / 9.0)
right_mid.set_mass(0.5 * m_link, 0.5 * I_link, 0.5 * I_link,
0.5 * I_link)
right_end = Frame(right, trep.TX, L_link / 3.0)
# Join the two links at the middle.
# trep.constraints.PointToPoint2D(system, 'xz', left_mid, right_mid)
# Add a new level. Note that left and right switch each time
return add_level(right_end, left_mid, link + 1)
# Create the new system
system = trep.System()
trep.potentials.Gravity(system, name="Gravity")
# Add free fall variable
drop = Frame(system.world_frame, trep.TZ, "DROP")
drop.config.q = 10. #L_link*math.cos(theta_0)
drop.set_mass(0.0)
wiggle = Frame(drop, trep.TY, "WIGGLE")
wiggle.config.q = 0. #L_link*math.cos(theta_0)
wiggle.set_mass(0.0)
# Add the top slider
slide = Frame(wiggle, trep.TX, "SLIDE")
slide.config.q = 0. #L_link*math.cos(theta_0)
slide.set_mass(0.0)
# Create all the links in the system.
add_level(slide, slide)
trep.forces.Damping(system, 0.0, {
'DROP': 0.,
'WIGGLE': 0.18,
'SLIDE': 0.18
})
trep.forces.BodyWrench(system,
slide,
wrench=(0., 0., 'forcez', 'moment1', 'moment2',
'moment3'))
# the passengers response motion
trep.forces.ConfigForce(system, 'PAX', 'response-torque') # Add input
system.satisfy_constraints()
return system
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()
# interpolated motor angles
# motor_angles = [qnew[1]-(np.pi/2), qnew[4]-(np.pi/2), qnew[6]-(np.pi/2)]
# motor_angles = [np.radians(-144.85), np.radians(-166.9), np.radians(-35.15)]
# (qU, qtrep, footPose) = geomFK(motor_angles, lengths, 1)
# print footPose
# angles = subchainIK(footPose, lengths)
# footPose[1] = -0.25
# print footPose
# final_angles = subchainIK(footPose, lengths)
# print final_angles
# footPose[1] = -0.18
# init_angles = subchainIK(footPose, lengths)
# print init_angles
# set up robot model in stance
hopper_stance = trep.System()
hopper_stance.import_frames([
# center of mass
tz('z_com', mass=0.001, name='CoM_robot'),
[
# middle links
tz(-0.1),
[
rx('motor1', name='motor1'),
[
tz(-0.0843, name='center_link1'),
[
rx('center_theta1'),
[
tz(-0.0856, name='center_link2'),
[
def load_urdf(root, system=None, prefix=None):
if system is None:
system = trep.System()
if prefix is None:
prefix = ''
links = root.findall('link')
joints = root.findall('joint')
link_dict = {}
for link in links:
inertial = link.find('inertial')
if inertial is not None:
inertia = inertial.find('inertia')
if inertia is not None:
inertia_dict = inertia.attrib
for attr in ['ixx','iyy','izz']:
if attr in inertia_dict:
inertia_dict[attr] = float(inertia_dict[attr])
else:
inertia_dict[attr] = 0.0
else:
inertia_dict = {'ixx':0.0, 'iyy':0.0, 'izz':0.0}
origin = inertial.find('origin')
if origin is not None:
origin_dict = origin.attrib
for attr in ['xyz', 'rpy']:
if attr in origin_dict:
values = str.split(origin_dict[attr])
origin_dict[attr] = [float(values[0]),float(values[1]),float(values[2])]
else:
origin_dict[attr] = [0.0, 0.0, 0.0]
else:
origin_dict = {'xyz':[0.0, 0.0, 0.0], 'rpy':[0.0, 0.0, 0.0]}
mass = inertial.find('mass')
if mass is not None:
mass_dict = mass.attrib
if 'value' in mass_dict:
mass_dict['value'] = float(mass_dict['value'])
else:
mass_dict['value'] = 0.0
else:
mass_dict = {'value':0.0}
inertial_dict = {'origin':origin_dict, 'mass':mass_dict, 'inertia':inertia_dict}
else:
inertial_dict = {'origin':{'xyz':[0.0, 0.0, 0.0], 'rpy':[0.0, 0.0, 0.0]}, 'mass':{'value':0.0}, 'inertia':{'ixx':0.0, 'iyy':0.0, 'izz':0.0}}
link_dict[link.get('name')] = inertial_dict
root_link = []
for link in links:
name = link.get('name')
ischild = False
for joint in joints:
if joint.find('child').get('link') == name:
ischild = True
break
if not ischild:
root_link.append(name)
frames = []
root_name = root_link[0]
# check if root is world and add all frames
if root_name == 'world':
add_child_frame(root_name, system.world_frame, link_dict, joints, prefix)
# else, must first create floating link to world_frame then add frames
else:
robot_name = root.get('name')
world_tx = trep.Frame(system.world_frame, trep.TX, prefix + robot_name +'-TX', name = prefix + robot_name +'-TX')
world_ty = trep.Frame(world_tx, trep.TY, prefix + robot_name +'-TY', name = prefix + robot_name +'-TY')
world_tz = trep.Frame(world_ty, trep.TZ, prefix + robot_name +'-TZ', name = prefix + robot_name +'-TZ')
world_rx = trep.Frame(world_tz, trep.RX, prefix + robot_name +'-RX', name = prefix + robot_name +'-RX')
world_ry = trep.Frame(world_rx, trep.RY, prefix + robot_name +'-RY', name = prefix + robot_name +'-RY')
root_frame = trep.Frame(world_ry, trep.RZ, prefix + robot_name +'-RZ', name = prefix + root_name)
# add mass to root frame
inertia_offset = link_dict[root_name]['origin']['xyz']
inertia_rotate = link_dict[root_name]['origin']['rpy']
if norm(inertia_offset) + norm(inertia_rotate) > 0.001:
# need to create link inertial frame
inertial_frame = trep.Frame(root_frame, trep.CONST_SE3, transform(inertia_offset, inertia_rotate), name = prefix + root_name + '-inertia')
inertial_frame.set_mass(link_dict[root_name]['mass']['value'], Ixx = link_dict[root_name]['inertia']['ixx'], Iyy = link_dict[root_name]['inertia']['iyy'], Izz = link_dict[root_name]['inertia']['izz'])
else:
root_frame.set_mass(link_dict[root_name]['mass']['value'], Ixx = link_dict[root_name]['inertia']['ixx'], Iyy = link_dict[root_name]['inertia']['iyy'], Izz = link_dict[root_name]['inertia']['izz'])
add_child_frame(root_name, root_frame, link_dict, joints, prefix)
# add damping to joint if specified
try:
damping = system.get_force('system-damping') #find damping if previously created
except KeyError:
damping = trep.forces.Damping(system, 0.0, name='system-damping') #create damping class if not
for joint in joints:
jdyn=joint.find('dynamics')
if jdyn is not None:
jdamp=jdyn.get('damping')
if jdamp is not None and system.get_config(prefix + joint.get('name')).kinematic is not True:
damping.set_damping_coefficient(prefix + joint.get('name'), float(jdamp))
return system
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()
