from pinocchio.utils import *
import numpy as np
from numpy import nan
import math
import time as tm
import eigenpy
import os
from base_controller.utils.common_functions import *
from base_controller.utils.optimTools import quadprog_solve_qp
from base_controller.utils.ros_publish import RosPub
from base_controller.utils.math_tools import Math
import ex_3_conf as conf
#instantiate graphic utils
os.system("killall rosmaster rviz")
ros_pub = RosPub("ur5")
robot = getRobotModel("ur5")
math_utils = Math()
# Init variables
zero = np.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0])
zero_cart = np.array([0.0, 0.0, 0.0])
time = 0.0
two_pi_f = 2 * np.pi * conf.freq # frequency (time 2 PI)
two_pi_f_amp = np.multiply(two_pi_f, conf.amp)
two_pi_f_squared_amp = np.multiply(two_pi_f, two_pi_f_amp)
# Init loggers
p_log = np.empty((3)) * nan
p_des_log = np.empty((3)) * nan
from pinocchio.utils import *
import numpy as np
from numpy import nan
import math
import time as tm
import eigenpy
import os
from base_controller.utils.common_functions import *
from base_controller.utils.optimTools import quadprog_solve_qp
from base_controller.utils.ros_publish import RosPub
import ex_5_conf as conf
#instantiate graphic utils
os.system("killall rosmaster")
ros_pub = RosPub("ur5")
robot = getRobotModel("ur5")
# Init variables
zero = np.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0])
time = 0.0
two_pi_f = 2 * np.pi * conf.freq # frequency (time 2 PI)
two_pi_f_amp = np.multiply(two_pi_f, conf.amp)
two_pi_f_squared_amp = np.multiply(two_pi_f, two_pi_f_amp)
# Init loggers
q_log = np.empty((6)) * nan
q_des_log = np.empty((6)) * nan
qd_log = np.empty((6)) * nan
qd_des_log = np.empty((6)) * nan
from pinocchio.utils import * import numpy as np from numpy import nan import math import time as tm import eigenpy eigenpy.switchToNumpyMatrix() import os from base_controller.utils.common_functions import * from base_controller.utils.optimTools import quadprog_solve_qp from base_controller.utils.ros_publish import RosPub import ex_3_conf as conf #instantiate graphic utils ros_pub = RosPub() robot = importDisplayModel(False, False) # Init variables zero = np.matrix([0.0, 0.0,0.0, 0.0, 0.0, 0.0]).T zero_cart = np.matrix([ 0.0, 0.0,0.0]).T time = 0.0 # Init loggers q_log = np.empty((6,0))*nan q_des_log = np.empty((6,0))*nan qd_log = np.empty((6,0))*nan qd_des_log = np.empty((6,0))*nan qdd_log = np.empty((6,0))*nan qdd_des_log = np.empty((6,0))*nan
import time as tm
import os
from base_controller.utils.common_functions import *
from base_controller.utils.ros_publish import RosPub
from base_controller.utils.kin_dyn_utils import RNEA
from base_controller.utils.kin_dyn_utils import getM
from base_controller.utils.kin_dyn_utils import getg
from base_controller.utils.kin_dyn_utils import getC
import ex_1_conf as conf
#instantiate graphic utils
os.system("killall rosmaster rviz")
ros_pub = RosPub("ur4")
robot = getRobotModel("ur4")
# Init variables
zero = np.array([0.0, 0.0, 0.0, 0.0])
time = 0.0
# Init loggers
q_log = np.empty((4))*nan
q_des_log = np.empty((4))*nan
qd_log = np.empty((4))*nan
qd_des_log = np.empty((4))*nan
qdd_log = np.empty((4))*nan
qdd_des_log = np.empty((4))*nan
tau_log = np.empty((4))*nan
def __init__(self):
#clean up previous process
os.system("killall gzserver gzclient")
if rosgraph.is_master_online(
): # Checks the master uri and results boolean (True or False)
print 'ROS MASTER is active'
nodes = rosnode.get_node_names()
if "/rviz" in nodes:
print("Rviz active")
rvizflag = " rviz:=false"
else:
rvizflag = " rviz:=true"
#start ros impedance controller
uuid = roslaunch.rlutil.get_or_generate_uuid(None, False)
roslaunch.configure_logging(uuid)
self.launch = roslaunch.parent.ROSLaunchParent(uuid, [
os.environ['LOCOSIM_DIR'] +
"/ros_impedance_controller/launch/ros_impedance_controller.launch"
])
#only available in ros lunar
# roslaunch_args=rvizflag
# self.launch = roslaunch.parent.ROSLaunchParent(uuid, [os.environ['LOCOSIM_DIR'] + "/ros_impedance_controller/launch/ros_impedance_controller_stdalone.launch"],roslaunch_args=[roslaunch_args])
self.launch.start()
ros.sleep(4.0)
threading.Thread.__init__(self)
# instantiate graphic utils
self.ros_pub = RosPub(True)
self.joint_names = ""
self.u = Utils()
self.comPoseW = np.zeros(6)
self.baseTwistW = np.zeros(6)
self.stance_legs = np.array([True, True, True, True])
self.q = np.zeros(12)
self.qd = np.zeros(12)
self.tau = np.zeros(12)
self.q_des = np.zeros(12)
self.qd_des = np.zeros(12)
self.tau_ffwd = np.zeros(12)
self.b_R_w = np.eye(3)
self.grForcesW = np.zeros(12)
self.basePoseW = np.zeros(6)
self.J = [np.eye(3)] * 4
self.wJ = [np.eye(3)] * 4
self.robot_name = ros.get_param('/robot_name')
self.sub_contact = ros.Subscriber("/" + self.robot_name +
"/contacts_state",
ContactsState,
callback=self._receive_contact,
queue_size=100)
self.sub_pose = ros.Subscriber("/" + self.robot_name + "/ground_truth",
Odometry,
callback=self._receive_pose,
queue_size=1)
self.sub_jstate = ros.Subscriber("/" + self.robot_name +
"/joint_states",
JointState,
callback=self._receive_jstate,
queue_size=1)
self.pub_des_jstate = ros.Publisher("/command",
JointState,
queue_size=1)
# freeze base and pause simulation service
self.reset_world = ros.ServiceProxy('/gazebo/set_model_state',
SetModelState)
self.reset_gravity = ros.ServiceProxy('/gazebo/set_physics_properties',
SetPhysicsProperties)
self.pause_physics_client = ros.ServiceProxy('/gazebo/pause_physics',
Empty)
self.unpause_physics_client = ros.ServiceProxy(
'/gazebo/unpause_physics', Empty)
# Loading a robot model of HyQ (Pinocchio)
self.robot = getRobotModel("hyq")
#send data to param server
self.verbose = conf.verbose
self.u.putIntoGlobalParamServer("verbose", self.verbose)