def encode_config(config, flat=True):
msg = ConfigMsg()
for k, v in config.items():
## @todo add more checks here?
if type(v) == int:
msg.ints.append(IntParameter(k, v))
elif type(v) == bool:
msg.bools.append(BoolParameter(k, v))
elif type(v) == str:
msg.strs.append(StrParameter(k, v))
elif sys.version_info.major < 3 and isinstance(v, unicode):
msg.strs.append(StrParameter(k, v))
elif type(v) == float:
msg.doubles.append(DoubleParameter(k, v))
elif type(v) == dict or isinstance(v, Config):
if flat is True:
def flatten(g):
groups = []
for _name, group in g['groups'].items():
groups.extend(flatten(group))
groups.append(GroupState(group['name'], group['state'], group['id'], group['parent']))
return groups
msg.groups.append(GroupState(v['name'], v['state'], v['id'], v['parent']))
msg.groups.extend(flatten(v))
else:
msg.groups = [GroupState(x['name'], x['state'], x['id'], x['parent']) for x in v]
return msg
def encode_config(config, flat=True):
msg = ConfigMsg()
for k, v in config.items():
## @todo add more checks here?
if type(v) == int:
msg.ints.append(IntParameter(k, v))
elif type(v) == bool:
msg.bools.append(BoolParameter(k, v))
elif type(v) == str:
msg.strs.append(StrParameter(k, v))
elif sys.version_info.major < 3 and isinstance(v, unicode):
msg.strs.append(StrParameter(k, v))
elif type(v) == float:
msg.doubles.append(DoubleParameter(k, v))
elif type(v) == dict or isinstance(v, Config):
if flat is True:
def flatten(g):
groups = []
for _name, group in g['groups'].items():
groups.extend(flatten(group))
groups.append(GroupState(group['name'], group['state'], group['id'], group['parent']))
return groups
msg.groups.append(GroupState(v['name'], v['state'], v['id'], v['parent']))
msg.groups.extend(flatten(v))
else:
msg.groups = [GroupState(x['name'], x['state'], x['id'], x['parent']) for x in v]
return msg
def generate_ros_msg(self):
msg = Config()
b = BoolParameter()
b.name = "antiwindup"
b.value = False
msg.bools = [b]
p = DoubleParameter()
p.name = "p"
p.value = self.p
i = DoubleParameter()
i.name = "i"
i.value = self.i
d = DoubleParameter()
d.name = "d"
d.value = self.d
msg.doubles = [p, i, d]
g = GroupState()
g.name = "Default"
g.state = True
g.id = 0
g.parent = 0
msg.groups = [g]
return msg
def update_velocity(self,u):
# print(self.Kp)
msg= Config()
self.Kp = self.callback(msg)[0]
self.Kd = self.callback(msg)[1]
u.twist.linear.x = self.Kp * (self.target_pos.pose.position.x - self.cur_pos.pose.position.x) + self.Kd * (
self.goal_vel - self.cur_vel.twist.linear.x)
u.twist.linear.y = self.Kp * (self.target_pos.pose.position.y - self.cur_pos.pose.position.y) + self.Kd * (
self.goal_vel - self.cur_vel.twist.linear.y)
u.twist.linear.z = self.Kp * (self.target_pos.pose.position.z - self.cur_pos.pose.position.z) + self.Kd * (
self.goal_vel - self.cur_vel.twist.linear.z)
if u.twist.linear.x > 2:
u.twist.linear.x = 2
elif u.twist.linear.x < -2:
u.twist.linear.x = -2
if u.twist.linear.y > 2:
u.twist.linear.y = 2
elif u.twist.linear.y < -2:
u.twist.linear.y = -2
if u.twist.linear.z > 1.5:
u.twist.linear.z = 1.5
elif u.twist.linear.z < -1.5:
u.twist.linear.z = -1.5
g_z_rot = self.trans_q_to_e(self.target_pos)
c_z_rot = self.trans_q_to_e(self.cur_pos)
u.twist.angular.z = self.Kp * (g_z_rot - c_z_rot) + self.Kd * (0.0 - self.cur_vel.twist.angular.z)
u.header.stamp = rospy.Time.now()
def set_parameters(self):
config_empty=Config()
velocity_scaling_param_tmp=DoubleParameter()
velocity_scaling_param_tmp.name='velocity_scaling'
velocity_scaling_param_tmp.value=self.velocity_scaling_goal
self.request.config.doubles.append(velocity_scaling_param_tmp)
self.set_parameters_client.call(self.request)
self.request.config=config_empty
def _create_parameter_updates_message(self, camera_position):
"""
Create a parameter_updates message to update the deprecated dynamic_reconigurable pan, tilt and zoom params.
:param camera_position: The camera position. Should contain keys 'pan', 'tilt', and probably also 'zoom'.
:type camera_position: dict
:return: The Config message.
:rtype: :py:class:`axis_camera.cfg.Config`
"""
message = Config()
message.doubles.append(
DoubleParameter(name="pan", value=camera_position['pan']))
message.doubles.append(
DoubleParameter(name="tilt", value=camera_position['tilt']))
message.ints.append(
IntParameter(name="zoom", value=camera_position['zoom']))
return message
def __init__(self):
pose_topic = "/mavros/local_position/pose"
rospy.Subscriber(pose_topic, PoseStamped, self.pose_sub)
velocity_topic = "/mavros/local_position/velocity_local"
rospy.Subscriber(velocity_topic, TwistStamped, self.velocity_sub)
rospy.Subscriber('/obstacles', PoseArray, self.obstacles_sub)
rospy.Subscriber(
"/listener/parameter_updates", Config,
self.dynamic_configure) # dynamics reconfigure subscriber
self.field_pub = rospy.Publisher('/field', TwistStamped, queue_size=10)
self.pose = PoseStamped()
self.velocity = TwistStamped()
self.obstacles = PoseArray()
self.config = Config()
self.field = TwistStamped()
def deserializeFromYaml(yaml_str):
cfg = yaml.load(yaml_str)
if not isinstance(cfg, dict):
return None
msg = Config()
if 'bools' in cfg and isinstance(cfg['bools'], list):
for p in cfg['bools']:
try:
msg.bools.append(BoolParameter(p['name'], p['value']))
except Exception:
pass
if 'ints' in cfg and isinstance(cfg['ints'], list):
for p in cfg['ints']:
try:
msg.ints.append(IntParameter(p['name'], p['value']))
except Exception:
pass
if 'strs' in cfg and isinstance(cfg['strs'], list):
for p in cfg['strs']:
try:
msg.strs.append(StrParameter(p['name'], p['value']))
except Exception:
pass
if 'doubles' in cfg and isinstance(cfg['doubles'], list):
for p in cfg['doubles']:
try:
msg.doubles.append(DoubleParameter(p['name'], p['value']))
except Exception:
pass
return msg
#!/usr/bin/env python3
# license removed for brevity
import rospy
from std_msgs.msg import Float64
import math
import random
import time
import numpy as np
from dynamic_reconfigure.msg import Config # dynamics reconfigure
config = Config() # dynamics reconfigure
def reconfigure_cb(data): # dynamics reconfigure callback
global config
config = data
class cheetah_control():
def __init__(self, type_of_control = 'position', time_pause_before_control = 5, rate_value = 50):
rospy.init_node('rotate', anonymous=True)
self.type_of_control = type_of_control
self.time_pause_before_control = time_pause_before_control
time.sleep(self.time_pause_before_control)
self.joints = {1: 'FL_hip_position_controller', 2: 'FL_thigh_position_controller', 3: 'FL_calf_position_controller',
4: 'FR_hip_position_controller', 5: 'FR_thigh_position_controller', 6: 'FR_calf_position_controller',
7: 'RL_hip_position_controller', 8: 'RL_thigh_position_controller', 9: 'RL_calf_position_controller',
def __init__(self):
# The current status of the joints.
self.JointState = JointTrajectoryControllerState()
# The servo power's status of the robot.
self.ServoPowerState = Bool()
# The fault power's status of the robot.
self.PowerFaultState = Bool()
# The reference coordinate in the calculations of the elfin_basic_api node
self.RefCoordinate = String()
# The end coordinate in the calculations of the elfin_basic_api node
self.EndCoordinate = String()
#The value of the dynamic parameters of elfin_basic_api, e.g. velocity scaling.
self.DynamicArgs = Config()
# get the reference coordinate name of elfin_basic_api from the response of this service.
self.call_ref_coordinate = rospy.ServiceProxy(
'elfin_basic_api/get_reference_link', SetBool)
self.call_ref_coordinate_req = SetBoolRequest()
# get the end coordinate name of elfin_basic_api from the response of this service.
self.call_end_coordinate = rospy.ServiceProxy(
'elfin_basic_api/get_end_link', SetBool)
self.call_end_coordinate_req = SetBoolRequest()
# for publishing joint goals to elfin_basic_api
self.JointsPub = rospy.Publisher('elfin_basic_api/joint_goal',
JointState,
queue_size=1)
self.JointsGoal = JointState()
# for publishing cart goals to elfin_basic_api
self.CartGoalPub = rospy.Publisher('elfin_basic_api/cart_goal',
PoseStamped,
queue_size=1)
self.CartPos = PoseStamped()
# for pub cart path
self.CartPathPub = rospy.Publisher('elfin_basic_api/cart_path_goal',
PoseArray,
queue_size=1)
self.CartPath = PoseArray()
self.CartPath.header.stamp = rospy.get_rostime()
self.CartPath.header.frame_id = 'elfin_base_link'
# for pub one specific joint action to elfin_teleop_joint_cmd_no_limit
self.JointCmdPub = rospy.Publisher('elfin_teleop_joint_cmd_no_limit',
Int64,
queue_size=1)
self.JointCmd = Int64()
# action client, send goal to move_group
self.action_client = SimpleActionClient(
'elfin_module_controller/follow_joint_trajectory',
FollowJointTrajectoryAction)
self.action_goal = FollowJointTrajectoryGoal()
#self.goal_list = JointTrajectoryPoint()
self.goal_list = []
self.joints_ = []
self.ps_ = []
self.listener = tf.TransformListener()
self.robot = moveit_commander.RobotCommander()
self.scene = moveit_commander.PlanningSceneInterface()
self.group = moveit_commander.MoveGroupCommander('elfin_arm')
self.ref_link_name = self.group.get_planning_frame()
self.end_link_name = self.group.get_end_effector_link()
self.ref_link_lock = threading.Lock()
self.end_link_lock = threading.Lock()
self.call_teleop_stop = rospy.ServiceProxy(
'elfin_basic_api/stop_teleop', SetBool)
self.call_teleop_stop_req = SetBoolRequest()
self.call_teleop_joint = rospy.ServiceProxy(
'elfin_basic_api/joint_teleop', SetInt16)
self.call_teleop_joint_req = SetInt16Request()
self.call_teleop_cart = rospy.ServiceProxy(
'elfin_basic_api/cart_teleop', SetInt16)
self.call_teleop_cart_req = SetInt16Request()
self.call_move_homing = rospy.ServiceProxy(
'elfin_basic_api/home_teleop', SetBool)
self.call_move_homing_req = SetBoolRequest()
self.call_reset = rospy.ServiceProxy(
self.elfin_driver_ns + 'clear_fault', SetBool)
self.call_reset_req = SetBoolRequest()
self.call_reset_req.data = True
self.call_power_on = rospy.ServiceProxy(
self.elfin_driver_ns + 'enable_robot', SetBool)
self.call_power_on_req = SetBoolRequest()
self.call_power_on_req.data = True
self.call_power_off = rospy.ServiceProxy(
self.elfin_driver_ns + 'disable_robot', SetBool)
self.call_power_off_req = SetBoolRequest()
self.call_power_off_req.data = True
self.call_velocity_setting = rospy.ServiceProxy(
'elfin_basic_api/set_velocity_scale', SetFloat64)
self.call_velocity_req = SetFloat64Request()
self._velocity_scale = 0.78
self.set_velocity_scale(self._velocity_scale)
pass
def pcodar_set_params(self, **kwargs):
result = yield self._pcodar_set_params(
ReconfigureRequest(Config(**kwargs)))
defer.returnValue(result)
but the real convention that is fed to robot is in randian scale
'''
def wait():
if do_wait:
print("Click enter to continue")
input()
do_wait = True
#callback function for robot pos
joints_position = Config() #variable for retrieving joints position
activate_joints = Config() #a boolean variable to activate joint space control
execute_trajectory = Config() #another boolean variable to excute desired trajectory
def pos_callback(robot_joints_pos):
global joints_position, activate_joints, execute_trajectory
joints_position = robot_joints_pos.doubles
activate_joints = robot_joints_pos.bools[1].value
execute_trajectory = robot_joints_pos.bools[0].value
'''
print("pose : \n",joints_position)
print("Condition", activate_joints)
print("Execute", execute_trajectory)'''
#callback function for robot pos in cartesian space
cartesian_pos = Config() #variable to retrieve cartesian position
def __init__(self,
type_of_control='position',
time_pause_before_control=0,
rate_value=1000,
use_ros=True):
self.use_ros = use_ros
if self.use_ros:
from std_msgs.msg import Float64
from sensor_msgs.msg import JointState
import rospy
from dynamic_reconfigure.msg import Config # dynamics reconfigure
rospy.init_node('cheetah_control', anonymous=True)
from gazebo_msgs.msg import LinkStates
self.type_of_control = type_of_control
self.time_pause_before_control = time_pause_before_control
time.sleep(self.time_pause_before_control)
# Declare the joint names of each leg
self.LB_leg = [
'left_back_hip_joint', 'left_back_thigh_joint',
'left_back_calf_joint'
]
self.RB_leg = [
'right_back_hip_joint', 'right_back_thigh_joint',
'right_back_calf_joint'
]
self.LF_leg = [
'left_forward_hip_joint', 'left_forward_thigh_joint',
'left_forward_calf_joint'
]
self.RF_leg = [
'right_forward_hip_joint', 'right_forward_thigh_joint',
'right_forward_calf_joint'
]
self.joints = {
'FL_hip': [self.LF_leg[0]],
'FL_thigh': [self.LF_leg[1]],
'FL_calf': [self.LF_leg[2]],
'FR_hip': [self.RF_leg[0]],
'FR_thigh': [self.RF_leg[1]],
'FR_calf': [self.RF_leg[2]],
'RL_hip': [self.LB_leg[0]],
'RL_thigh': [self.LB_leg[1]],
'RL_calf': [self.LB_leg[2]],
'RR_hip': [self.RB_leg[0]],
'RR_thigh': [self.RB_leg[1]],
'RR_calf': [self.RB_leg[2]]
}
self.joints_to_corresp = copy.deepcopy(self.joints)
if self.use_ros:
if type_of_control == 'position':
self.controllers = list(
map(lambda x: x + '_position_controller', self.joints))
elif type_of_control == 'torque':
self.controllers = list(
map(lambda x: x + '_Effort_controller', self.joints))
self.pub = {}
i = 0
for joint in self.joints:
self.joints[joint].append(self.controllers[i])
self.pub[joint] = rospy.Publisher(self.joint_name(
self.joints[joint][1]),
Float64,
queue_size=10)
i += 1
self.cheetah_joints_sub = rospy.Subscriber(
"/quadruped/joint_states", JointState,
self.joints_pos_callback)
self.cheetah_links_sub = rospy.Subscriber("/gazebo/link_states",
LinkStates,
self.body_pos_callback)
self.rate = rospy.Rate(rate_value)
self.joints_positions = {}
self.joints_velocities = {}
self.body_position = {}
self.body_orientation = {}
self.body_twist_angular = {}
self.body_twist_linear = {}
self.reconfigure_sub = rospy.Subscriber(
'/dynamic_reconfigure/parameter_updates', Config,
self.reconfigure_cb) # dynamics reconfigure subscriber
self.config = Config() # dynamics reconfigure
from dynamic_tutorials.cfg import TutorialsConfig
from mavros_msgs.msg import State, ExtendedState
from tf.transformations import euler_from_quaternion
from dynamic_reconfigure.server import Server
from dynamic_reconf.cfg import DynamicConfig
from mavros_msgs.msg import State, ExtendedState
from dynamic_reconfigure.msg import Config
mavros.set_namespace()
pose = PoseStamped()
velocity = TwistStamped()
obstacles = PoseArray()
config = Config()
field = TwistStamped()
# pose.pose.position
def pose_sub(msg):
global pose
pose = msg.pose.position
def velocity_sub(msg):
global velocity
velocity = msg
def obstacles_sub(msg):