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)
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)
def build_leg(self, attach, n, L, l):
# upper leg:
child = trep.Frame(attach, trep.RX, "leg_%d_th1" % n, "leg_%d_th1" % n)
child = trep.Frame(child,
trep.TY,
-L / 2.,
"leg_%d_upper_inertial" % n,
mass=.10)
child = trep.Frame(child, trep.TY, -L / 2., "leg_%d_upper_end" % n)
# first universal joint:
child = trep.Frame(child, trep.RX, "leg_%d_th2" % n, "leg_%d_th2" % n)
child = trep.Frame(child, trep.RZ, "leg_%d_th3" % n, "leg_%d_th3" % n)
# lower leg:
child = trep.Frame(child,
trep.TY,
-l / 2,
"leg_%d_lower_inertial" % n,
mass=.07)
child = trep.Frame(child, trep.TY, -l / 2., "leg_%d_lower_end" % n)
# second universal joint:
child = trep.Frame(child, trep.RZ, "leg_%d_th4" % n, "leg_%d_th4" % n)
child = trep.Frame(child, trep.RX, "leg_%d_th5" % n, "leg_%d_end" % n)
return
def import_frames(self, children):
"""
Import a tree of frames from a tree description.
"""
self.system.hold_structure_changes()
while children:
info = children[0]
children = children[1:]
if not isinstance(info, trep.frame.FrameDef):
raise TypeError('Frame definition expected instead of: %r', info)
frame = trep.Frame(self, info.transform_type, info.param,
name=info.name,
kinematic=info.kinematic,
mass=info.mass)
if children and isinstance(children[0], list):
frame.import_frames(children[0])
children = children[1:]
self.system.resume_structure_changes()
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 __init__(self, L_base, L_platform, R_base, R_platform):
# first let's create an empty system, and build the base platform:
super(DeltaRobotTrep, self).__init__()
# triangle sizes:
s_base = R_base * 6. / np.sqrt(3)
s_platform = R_platform * 6. / np.sqrt(3)
# center of circles to platform vertices:
u_base = s_base * np.sqrt(3) / 3.
u_platform = s_platform * np.sqrt(3) / 3.
# center of circles to triangle edges
w_base = R_base
w_platform = R_platform
# LEG 1 ATTACHMENT
_ = trep.Frame(self.world_frame, trep.TY, -w_base, "base_attach_leg1")
# LEG 2 ATTACHMENT
child = trep.Frame(self.world_frame, trep.TX,
w_base * np.cos(30 * pi / 180.))
child = trep.Frame(child, trep.TY, w_base * np.sin(30 * pi / 180.))
_ = trep.Frame(child, trep.RZ, 120 * pi / 180., "base_attach_leg2")
# LEG 3 ATTACHMENT
child = trep.Frame(self.world_frame, trep.TX,
-w_base * np.cos(30 * pi / 180.))
child = trep.Frame(child, trep.TY, w_base * np.sin(30 * pi / 180.))
_ = trep.Frame(child, trep.RZ, 240 * pi / 180., "base_attach_leg3")
# BUILD LEGS
self.build_leg(self.get_frame("base_attach_leg1"), 1, L_base,
L_platform)
self.build_leg(self.get_frame("base_attach_leg2"), 2, L_base,
L_platform)
self.build_leg(self.get_frame("base_attach_leg3"), 3, L_base,
L_platform)
# ADD PLATFORM AND ATTACHMENTS:
child = trep.Frame(self.world_frame, trep.TX, "platform_x",
"platform_x")
child = trep.Frame(child, trep.TY, "platform_y", "platform_y")
self.platform_center = trep.Frame(child,
trep.TZ,
"platform_z",
"platform_z",
mass=.15)
_ = trep.Frame(self.platform_center, trep.TY, -u_platform,
"platform_attach_leg1")
child = trep.Frame(self.platform_center, trep.TX,
u_platform * np.cos(30 * pi / 180.))
child = trep.Frame(child, trep.TY, u_platform * np.sin(30 * pi / 180.))
_ = trep.Frame(child, trep.RZ, 120 * pi / 180., "platform_attach_leg2")
child = trep.Frame(self.platform_center, trep.TX,
-u_platform * np.cos(30 * pi / 180.))
child = trep.Frame(child, trep.TY, u_platform * np.sin(30 * pi / 180.))
_ = trep.Frame(child, trep.RZ, 240 * pi / 180., "platform_attach_leg3")
# NOW ADD CONSTRAINTS
trep.constraints.PointToPoint3D(self, "platform_attach_leg1",
"leg_1_end")
trep.constraints.PointToPoint3D(self, "platform_attach_leg2",
"leg_2_end")
trep.constraints.PointToPoint3D(self, "platform_attach_leg3",
"leg_3_end")
# ADD SPRINGS
q_neutral = 22.5 * pi / 180.
stiffness = 10
trep.potentials.ConfigSpring(self,
'leg_1_th1',
k=stiffness,
q0=q_neutral)
trep.potentials.ConfigSpring(self,
'leg_2_th1',
k=stiffness,
q0=q_neutral)
trep.potentials.ConfigSpring(self,
'leg_3_th1',
k=stiffness,
q0=q_neutral)
# ADD GRAVITY AND DAMPING
trep.potentials.Gravity(self, gravity=[0, 0, -9.8], name="gravity")
trep.forces.Damping(self, 0.05)
return
def add_child_frame(parent_name, parent_frame, links, joints, prefix):
for joint in joints:
if joint.find('parent').get('link') == parent_name:
child_name = joint.find('child').get('link')
joint_name = joint.get('name')
# check if joint should be kinematically controlled
if joint.get('kinematic') == 'True':
iskin = True
else:
iskin = False
# First create new joint frame if origin tag is specified
j_origin = joint.find('origin')
if j_origin == None:
joint_frame = parent_frame
else:
if j_origin.get('xyz') is not None:
j_xyz = str.split(j_origin.get('xyz'))
else:
j_xyz = [0.0, 0.0, 0.0]
if j_origin.get('rpy') is not None:
j_rpy = str.split(j_origin.get('rpy'))
else:
j_rpy = [0.0, 0.0, 0.0]
origin = transform([float(j_xyz[0]),float(j_xyz[1]),float(j_xyz[2])],[float(j_rpy[0]),float(j_rpy[1]),float(j_rpy[2])])
joint_frame = trep.Frame(parent_frame, trep.CONST_SE3, origin, name = prefix + joint_name)
# Fixed Joint
if joint.get('type') == 'fixed':
child_frame = joint_frame
child_frame.name = prefix + child_name
# Continuous Joint
elif joint.get('type') == 'continuous':
j_axis = joint.find('axis')
if j_axis == None:
j_xyz = [1, 0, 0]
else:
j_xyz = str.split(j_axis.get('xyz'))
j_xyz = [float(j_xyz[0]), float(j_xyz[1]), float(j_xyz[2])]
j_xyz /= norm(j_xyz)
if np.dot(j_xyz, [1,0,0]) > 0.99:
#rotate about x-axis
child_frame = trep.Frame(joint_frame, trep.RX, prefix + joint_name, name = prefix + child_name, kinematic = iskin)
elif np.dot(j_xyz, [0,1,0]) > 0.99:
#rotate about y-axis
child_frame = trep.Frame(joint_frame, trep.RY, prefix + joint_name, name = prefix + child_name, kinematic = iskin)
elif np.dot(j_xyz, [0,0,1]) > 0.99:
#rotate about z-axis
child_frame = trep.Frame(joint_frame, trep.RZ, prefix + joint_name, name = prefix + child_name, kinematic = iskin)
else:
#arbitray axis rotation
rotation_mat = rotate(j_xyz)
rotation_frame = trep.Frame(joint_frame, trep.CONST_SE3, np.vstack((rotation_mat,[0,0,0])), name = prefix + child_name + '-axis')
config_frame = trep.Frame(rotation_frame, trep.RX, prefix + joint_name, name = prefix + joint_name + '-axis', kinematic = iskin)
child_frame = trep.Frame(config_frame, trep.CONST_SE3, np.vstack((np.transpose(rotation_mat),[0,0,0])), name = prefix + child_name)
# Prismatic Joint
elif joint.get('type') == 'prismatic':
j_axis = joint.find('axis')
if j_axis == None:
j_xyz = [1, 0, 0]
else:
j_xyz = str.split(j_axis.get('xyz'))
j_xyz = [float(j_xyz[0]), float(j_xyz[1]), float(j_xyz[2])]
j_xyz /= norm(j_xyz)
if np.dot(j_xyz, [1,0,0]) > 0.99:
#translate on x-axis
child_frame = trep.Frame(joint_frame, trep.TX, prefix + joint_name, name = prefix + child_name, kinematic = iskin)
elif np.dot(j_xyz, [0,1,0]) > 0.99:
#translate on y-axis
child_frame = trep.Frame(joint_frame, trep.TY, prefix + joint_name, name = prefix + child_name, kinematic = iskin)
elif np.dot(j_xyz, [0,0,1]) > 0.99:
#translate on z-axis
child_frame = trep.Frame(joint_frame, trep.TZ, prefix + joint_name, name = prefix + child_name, kinematic = iskin)
else:
#arbitray axis translation
rotation_mat = rotate(j_xyz)
translation_frame = trep.Frame(joint_frame, trep.CONST_SE3, np.vstack((rotation_mat,[0,0,0])), name = prefix + child_name + '-axis')
config_frame = trep.Frame(translation_frame, trep.TX, prefix + joint_name, name = prefix + joint_name + '-axis', kinematic = iskin)
child_frame = trep.Frame(config_frame, trep.CONST_SE3, np.vstack((np.transpose(rotation_mat),[0,0,0])), name = prefix + child_name)
# Floating Joint - may not enable since robot_state_pubisher doesn't support
# elif joint.get('type') == 'floating':
# tx_frame = trep.Frame(joint_frame, trep.TX, joint_name + '-TX')
# ty_frame = trep.Frame(tx_frame, trep.TY, joint_name + '-TY')
# tz_frame = trep.Frame(ty_frame, trep.TZ, joint_name + '-TZ')
# rx_frame = trep.Frame(tz_frame, trep.RX, joint_name + '-RX')
# ry_frame = trep.Frame(rx_frame, trep.RY, joint_name + '-RY')
# child_frame = trep.Frame(ry_frame, trep.RZ, child_name)
# No match for joint type
else:
print "Invalid joint type specified. Exiting..."
break
# add mass to child link
inertia_offset = links[child_name]['origin']['xyz']
inertia_rotate = links[child_name]['origin']['rpy']
if norm(inertia_offset) + norm(inertia_rotate) > 0.001:
# need to create link inertial frame
inertial_frame = trep.Frame(child_frame, trep.CONST_SE3, transform(inertia_offset, inertia_rotate), name = prefix + child_name + '-inertia')
inertial_frame.set_mass(links[child_name]['mass']['value'], Ixx = links[child_name]['inertia']['ixx'], Iyy = links[child_name]['inertia']['iyy'], Izz = links[child_name]['inertia']['izz'])
else:
child_frame.set_mass(links[child_name]['mass']['value'], Ixx = links[child_name]['inertia']['ixx'], Iyy = links[child_name]['inertia']['iyy'], Izz = links[child_name]['inertia']['izz'])
# Start child node recursion
add_child_frame(child_name, child_frame, links, joints, prefix)
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()