def set_joint_angle(model_name: str, joint_names: list, angles: list):
# assert 0 <= angle <= np.pi / 2, angle
assert len(joint_names) == len(angles)
set_model_configuration_srv = rospy.ServiceProxy(
"gazebo/set_model_configuration", SetModelConfiguration)
req = SetModelConfigurationRequest()
req.model_name = model_name
# req.urdf_param_name = 'robot_description'
req.joint_names = joint_names # list
req.joint_positions = angles # list
res = set_model_configuration_srv(req)
assert res.success, res
return res
def reset_joints(self, joints_list, model_name):
config = SetModelConfigurationRequest()
config.model_name = model_name
config.urdf_param_name = 'robot_description'
config.joint_names = joints_list
config.joint_positions = np.zeros(len(config.joint_names))
self.model_config_proxy(config)
def set_initial_pose_gazebo():
set_model_config_client = rospy.ServiceProxy(SET_MODEL_CONFIGURATION, SetModelConfiguration)
req = SetModelConfigurationRequest()
req.model_name = "ur5"
req.urdf_param_name = "robot_description"
req.joint_names = rospy.get_param("/initial_pose/joint_names")
req.joint_positions = rospy.get_param("/initial_pose/joint_values")
rospy.wait_for_service(SET_MODEL_CONFIGURATION)
res = set_model_config_client(req)
def move_to_joint_configs(self, controller_name, angles):
set_model_config_client = rospy.ServiceProxy(SET_MODEL_CONFIGURATION, SetModelConfiguration)
req = SetModelConfigurationRequest()
req.model_name = "pr2"
req.urdf_param_name = "robot_description"
req.joint_names = rospy.get_param("/%s/joints" % (controller_name))
req.joint_positions = angles
rospy.wait_for_service(SET_MODEL_CONFIGURATION)
res = set_model_config_client(req)
def _set_model_config(self):
rospy.wait_for_service('/gazebo/set_model_configuration')
set_model_srv = rospy.ServiceProxy('/gazebo/set_model_configuration',
SetModelConfiguration)
model_config = SetModelConfigurationRequest()
model_config.model_name = 'j2n6s300'
model_config.urdf_param_name = '/robot_description'
model_config.joint_names = self.joint_names
model_config.joint_positions = self.init_pose
result = set_model_srv(model_config)
def set_joints(self, array=[1.5, -1.2, 1.4, -1.87, -1.57, 0]):
reset_req = SetModelConfigurationRequest()
reset_req.model_name = 'pickbot'
reset_req.urdf_param_name = 'robot_description'
reset_req.joint_names = [
'elbow_joint', 'shoulder_lift_joint', 'shoulder_pan_joint',
'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint'
]
reset_req.joint_positions = array
res = self.reset_joints(reset_req)
def set_config_request(self, joint_names=None, joint_positions=None):
# Create service requests for the set config and switch controller services.
self.config_request = SetModelConfigurationRequest()
self.config_request.model_name = "baxter"
self.config_request.urdf_param_name = "robot_description"
# Set default joint names and joint positions.
# Can also set things for left arm by just providing corresponding dictionary.
if (joint_names is None) and (joint_positions is None):
if self.arm == "right":
self.config_request.joint_names = [
"right_s0", "right_s1", "right_e0", "right_e1", "right_w0",
"right_w1", "right_w2"
]
elif self.arm == "left":
self.config_request.joint_names = [
"left_s0", "left_s1", "left_e0", "left_e1", "left_w0",
"left_w1", "left_w2"
]
self.config_request.joint_positions = [0., 0., 0., 0., 0., 0., 0.]
if joint_names is not None:
self.config_request.joint_names = joint_names
if joint_positions is not None:
self.config_request.joint_positions = joint_positions
def resetJoints(self, init_angles):
reset_req = SetModelConfigurationRequest()
reset_req.model_name = "laelaps"
reset_req.urdf_param_name = 'robot_description'
reset_req.joint_names = [
"RF_knee", "RF_hip", "RH_knee", "RH_hip", "LF_knee", "LF_hip",
"LH_knee", "LH_hip"
]
reset_req.joint_positions = init_angles
rospy.wait_for_service('/gazebo/set_model_configuration')
try:
#reset_proxy.call()
# reset_world()
self.reset_joints(reset_req)
# print("Reset Joints successed!!")
# self.reset_joints("minitaur", "robot_description", ['0', '1', '2', '3', '4', '5','6','7'], [0.0,0.0,0.0, 0.0, 0.0, 0.0,0.0,0.0])
except (rospy.ServiceException) as e:
print("/gazebo/reset_joints service call failed")
def __init__(self):
rospy.init_node('joint_position_node')
self.nb_joints = 12
self.nb_links = 13
self.state_shape = (self.nb_joints * 2 + 4 + 3 + 3,
) #joint states + orientation
self.action_shape = (self.nb_joints, )
self.link_name_lst = [
'quadruped::base_link', 'quadruped::front_right_leg1',
'quadruped::front_right_leg2', 'quadruped::front_right_leg3',
'quadruped::front_left_leg1', 'quadruped::front_left_leg2',
'quadruped::front_left_leg3', 'quadruped::back_right_leg1',
'quadruped::back_right_leg2', 'quadruped::back_right_leg3',
'quadruped::back_left_leg1', 'quadruped::back_left_leg2',
'quadruped::back_left_leg3'
]
self.leg_link_name_lst = self.link_name_lst[1:]
self.joint_name_lst = [
'front_right_leg1_joint', 'front_right_leg2_joint',
'front_right_leg3_joint', 'front_left_leg1_joint',
'front_left_leg2_joint', 'front_left_leg3_joint',
'back_right_leg1_joint', 'back_right_leg2_joint',
'back_right_leg3_joint', 'back_left_leg1_joint',
'back_left_leg2_joint', 'back_left_leg3_joint'
]
self.all_joints = AllJoints(self.joint_name_lst)
self.starting_pos = np.array([
-0.01, 0.01, 0.01, -0.01, 0.01, -0.01, -0.01, 0.01, -0.01, -0.01,
0.01, 0.01
])
self.pause_proxy = rospy.ServiceProxy('/gazebo/pause_physics', Empty)
self.unpause_proxy = rospy.ServiceProxy('/gazebo/unpause_physics',
Empty)
self.model_config_proxy = rospy.ServiceProxy(
'/gazebo/set_model_configuration', SetModelConfiguration)
self.model_config_req = SetModelConfigurationRequest()
self.model_config_req.model_name = 'quadruped'
self.model_config_req.urdf_param_name = 'robot_description'
self.model_config_req.joint_names = self.joint_name_lst
self.model_config_req.joint_positions = self.starting_pos
self.model_state_proxy = rospy.ServiceProxy('/gazebo/set_model_state',
SetModelState)
self.model_state_req = SetModelStateRequest()
self.model_state_req.model_state = ModelState()
self.model_state_req.model_state.model_name = 'quadruped'
self.model_state_req.model_state.pose.position.x = 0.0
self.model_state_req.model_state.pose.position.y = 0.0
self.model_state_req.model_state.pose.position.z = 0.25
self.model_state_req.model_state.pose.orientation.x = 0.0
self.model_state_req.model_state.pose.orientation.y = 0.0
self.model_state_req.model_state.pose.orientation.z = 0.0
self.model_state_req.model_state.pose.orientation.w = 0.0
self.model_state_req.model_state.twist.linear.x = 0.0
self.model_state_req.model_state.twist.linear.y = 0.0
self.model_state_req.model_state.twist.linear.z = 0.0
self.model_state_req.model_state.twist.angular.x = 0.0
self.model_state_req.model_state.twist.angular.y = 0.0
self.model_state_req.model_state.twist.angular.z = 0.0
self.model_state_req.model_state.reference_frame = 'world'
self.get_model_state_proxy = rospy.ServiceProxy(
'/gazebo/get_model_state', GetModelState)
self.get_model_state_req = GetModelStateRequest()
self.get_model_state_req.model_name = 'quadruped'
self.get_model_state_req.relative_entity_name = 'world'
self.last_pos = np.zeros(3)
self.last_ori = np.zeros(4)
self.joint_pos_high = np.array(
[1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0])
self.joint_pos_low = np.array([
-1.0, -1.0, -1.0, -1.0, -1.0, -1.0, -1.0, -1.0, -1.0, -1.0, -1.0,
-1.0
])
self.joint_pos_coeff = 3.0
self.joint_pos_range = self.joint_pos_high - self.joint_pos_low
self.joint_pos_mid = (self.joint_pos_high + self.joint_pos_low) / 2.0
self.joint_pos = self.starting_pos
self.joint_state = np.zeros(self.nb_joints)
self.joint_state_subscriber = rospy.Subscriber(
'/quadruped/joint_states', JointState,
self.joint_state_subscriber_callback)
self.orientation = np.zeros(4)
self.angular_vel = np.zeros(3)
self.linear_acc = np.zeros(3)
self.imu_subscriber = rospy.Subscriber('/quadruped/imu', Imu,
self.imu_subscriber_callback)
self.normed_sp = self.normalize_joint_state(self.starting_pos)
self.state = np.zeros(self.state_shape)
self.diff_state_coeff = 4.0
self.reward_coeff = 20.0
self.reward = 0.0
self.done = False
self.episode_start_time = 0.0
self.max_sim_time = 15.0
self.pos_z_limit = 0.18
self.action_coeff = 1.0
self.linear_acc_coeff = 0.1
self.last_action = np.zeros(self.nb_joints)
def __init__(self):
rospy.init_node('RL_Node')
# TODO: Pile information
self.pile = Pile() # (1.75, 2.8, 1.05, 0.34)
self.pile.length = 1.75
self.pile.width = 2.8
self.pile.height = 1.05
self.pile.size = 0.34
self.pile_flag = True
# TODO: Robot information
self.bucket_init_pos = 0
self.arm_init_pos = 0
self.vel_init = 0
self.HALF_KOMODO = 0.53 / 2
self.particle = 0
self.x_tip = 0
self.z_tip = 0
self.bucket_link_x = 0
self.bucket_link_z = 0
self.velocity = 0
self.wheel_vel = 0
self.joint_name_lst = [
'arm_joint', 'bucket_joint', 'front_left_wheel_joint',
'front_right_wheel_joint', 'rear_left_wheel_joint',
'rear_right_wheel_joint'
]
self.last_pos = np.zeros(3)
self.last_ori = np.zeros(4)
self.max_limit = np.array([0.1, 0.32, 0.9])
self.min_limit = np.array([-0.1, -0.1, -0.5])
self.orientation = np.zeros(3)
self.angular_vel = np.zeros(3)
self.linear_acc = np.zeros(3)
# TODO: RL information
self.nb_actions = 3 # base , arm , bucket
self.state_shape = (self.nb_actions * 2 + 6, )
self.action_shape = (self.nb_actions, )
self.actions = Actions()
self.starting_pos = np.array(
[self.vel_init, self.arm_init_pos, self.bucket_init_pos])
self.action_range = self.max_limit - self.min_limit
self.action_mid = (self.max_limit + self.min_limit) / 2.0
self.last_action = np.zeros(self.nb_actions)
self.joint_state = np.zeros(self.nb_actions)
self.joint_pos = self.starting_pos
self.state = np.zeros(self.state_shape)
self.reward = 0.0
self.done = False
self.reward_done = False
self.reach_target = False
self.episode_start_time = 0.0
self.max_sim_time = 8.0
# TODO: Robot information Subscribers
self.joint_state_subscriber = rospy.Subscriber(
'/joint_states', JointState, self.joint_state_subscriber_callback)
self.velocity_subscriber = rospy.Subscriber(
'/mobile_base_controller/odom', Odometry,
self.velocity_subscriber_callback)
self.imu_subscriber = rospy.Subscriber('/IMU', Imu,
self.imu_subscriber_callback)
self.reward_publisher = rospy.Publisher('/reward',
Float64,
queue_size=10)
# TODO: Torque related
# self.torque_subscriber = rospy.Subscriber('/bucket_torque_sensor',WrenchStamped,self.torque_subscriber_callback)
# self.controller_state_sub = rospy.Subscriber('/bucket_position_controller/state',JointControllerState,self.controller_subscriber_callback)
# self.torque_publisher = rospy.Publisher('/t1',Float64,queue_size=10)
# self.controller_publisher = rospy.Publisher('/t2',Float64,queue_size=10)
#
# self.torque_sum = 0
# self.torque_y = 0
# self.smooth_command = 0
# self.ft_out = WrenchStamped()
# self.ft_out.header.stamp = rospy.Time.now()
# self.ft_out.wrench.force.x = 0
# self.ft_out.wrench.force.y = 0
# self.ft_out.wrench.force.z = 0
# self.ft_out.wrench.torque.x = 0
# self.ft_out.wrench.torque.y = 0
# self.ft_out.wrench.torque.z = 0
# TODO: Gazebo stuff
self.pause_proxy = rospy.ServiceProxy('/gazebo/pause_physics', Empty)
self.unpause_proxy = rospy.ServiceProxy('/gazebo/unpause_physics',
Empty)
self.reset_world = rospy.ServiceProxy('/gazebo/reset_world', Empty)
self.tfl = TransformListener()
self.model_config_proxy = rospy.ServiceProxy(
'/gazebo/set_model_configuration', SetModelConfiguration)
self.model_config_req = SetModelConfigurationRequest()
self.model_config_req.model_name = 'komodo2'
self.model_config_req.urdf_param_name = 'robot_description'
self.model_config_req.joint_names = self.joint_name_lst
self.model_config_req.joint_positions = self.starting_pos
self.model_state_proxy = rospy.ServiceProxy('/gazebo/set_model_state',
SetModelState)
self.model_state_req = SetModelStateRequest()
self.model_state_req.model_state = ModelState()
self.model_state_req.model_state.model_name = 'komodo2'
self.model_state_req.model_state.pose.position.x = 1.0
self.model_state_req.model_state.pose.position.y = 0.0
self.model_state_req.model_state.pose.position.z = 0.0
self.model_state_req.model_state.pose.orientation.x = 0.0
self.model_state_req.model_state.pose.orientation.y = 0.0
self.model_state_req.model_state.pose.orientation.z = 0.0
self.model_state_req.model_state.pose.orientation.w = 0.0
self.model_state_req.model_state.twist.linear.x = 0.0
self.model_state_req.model_state.twist.linear.y = 0.0
self.model_state_req.model_state.twist.linear.z = 0.0
self.model_state_req.model_state.twist.angular.x = 0.0
self.model_state_req.model_state.twist.angular.y = 0.0
self.model_state_req.model_state.twist.angular.z = 0.0
self.model_state_req.model_state.reference_frame = 'world'
self.get_model_state_proxy = rospy.ServiceProxy(
'/gazebo/get_model_state', GetModelState)
self.get_model_state_req = GetModelStateRequest()
self.get_model_state_req.model_name = 'komodo2'
self.get_model_state_req.relative_entity_name = 'world'
self.get_link_state_proxy = rospy.ServiceProxy(
'/gazebo/get_link_state', GetLinkState)
self.get_link_state_req = GetLinkStateRequest()
self.get_link_state_req.link_name = 'bucket'
self.get_link_state_req.reference_frame = 'world'
import rospy
from controller_manager_msgs.srv import SwitchController, SwitchControllerRequest
from gazebo_msgs.srv import SetModelState, SetModelStateRequest, SetModelConfiguration, SetModelConfigurationRequest
import numpy as np
rospy.init_node("test")
joint_names = ['plat_joint','shoulder_joint','forearm_joint','wrist_joint']
starting_pos = np.array([0, 0, 1, 2])
model_config_proxy = rospy.ServiceProxy('/gazebo/set_model_configuration',SetModelConfiguration)
model_config_req = SetModelConfigurationRequest()
model_config_req.model_name = 'arm'
model_config_req.urdf_param_name = 'robot_description'
model_config_req.joint_names = joint_names
model_config_req.joint_positions = starting_pos
controller_switch_proxy = rospy.ServiceProxy('/controller_manager/switch_controller',SwitchController)
controller_switch_off = SwitchControllerRequest()
controller_switch_off.stop_controllers = ['joint_state_controller','arm_controller']
controller_switch_off.strictness = 2
controller_switch_on = SwitchControllerRequest()
controller_switch_on.start_controllers = ['joint_state_controller','arm_controller']
controller_switch_on.strictness = 2
rospy.wait_for_service('/controller_manager/switch_controller')
try:
controller_switch_proxy(controller_switch_off)
except rospy.ServiceException, e:
print('/controller_manager/switch_controller call failed')
#!/usr/bin/env python
# coding: utf-8
import roslib
import rospy
from gazebo_msgs.srv import SetModelConfiguration
from gazebo_msgs.srv import SetModelConfigurationRequest
from std_srvs.srv import Empty
rospy.wait_for_service('/gazebo/unpause_physics', timeout=120)
rospy.wait_for_service('/gazebo/set_model_configuration', timeout=120)
unpause_physics = rospy.ServiceProxy('/gazebo/unpause_physics', Empty)
initialize_joints = rospy.ServiceProxy('/gazebo/set_model_configuration', SetModelConfiguration)
rospy.sleep(5.0)
pose_req = SetModelConfigurationRequest()
pose_req.model_name = 'sciurus17'
pose_req.urdf_param_name = '/sciurus17/robot_description'
pose_req.joint_names = [ 'waist_yaw_joint', 'neck_yaw_joint', 'neck_pitch_joint', \
'r_arm_joint1', 'r_arm_joint2', 'r_arm_joint3', 'r_arm_joint4', 'r_arm_joint5', 'r_arm_joint6', 'r_arm_joint7', 'l_hand_joint', \
'l_arm_joint1', 'l_arm_joint2', 'l_arm_joint3', 'l_arm_joint4', 'l_arm_joint5', 'l_arm_joint6', 'l_arm_joint7', 'r_hand_joint' ]
pose_req.joint_positions = [ 0.0, 0.0, 0.0, \
0.0, -1.5, 0.0, 2.1, 0.0, -2.0, 0.0, 0.0, \
0.0, 1.5, 0.0, -2.1, 0.0, 2.0, 0.0, 0.0 ]
res = initialize_joints( pose_req )
res = unpause_physics()
controllers_list = [
'joint_1_position_controller', 'joint_2_position_controller',
'joint_3_position_controller', 'joint_4_position_controller',
'joint_5_position_controller', 'joint_6_position_controller',
'finger_1_position_controller', 'finger_2_position_controller',
'finger_3_position_controller', 'joint_state_controller'
]
joint_names = [
'j2n6s300_joint_1', 'j2n6s300_joint_2', 'j2n6s300_joint_3',
'j2n6s300_joint_4', 'j2n6s300_joint_5', 'j2n6s300_joint_6',
'j2n6s300_finger_1', 'j2n6s300_finger_2', 'j2n6s300_finger_3'
]
joint_positions = [0.0, 2.9, 1.3, -2.07, 1.4, 0.0, 1, 1, 1]
model_config = SetModelConfigurationRequest()
model_config.model_name = 'j2n6s300'
model_config.urdf_param_name = '/robot_description'
model_config.joint_names = joint_names
model_config.joint_positions = joint_positions
controllers_object = ControllersConnection(namespace='j2n6s300',
controllers_list=controllers_list)
pub = rospy.Publisher('/j2n6s300/effort_joint_trajectory_controller/command',
JointTrajectory,
queue_size=1)
pubf = rospy.Publisher('/j2n6s300/effort_joint_trajectory_controller/command',
JointTrajectory,
queue_size=1)
unpause_gazebo = rospy.ServiceProxy('/gazebo/unpause_physics', Empty)
pause_gazebo = rospy.ServiceProxy('/gazebo/pause_physics', Empty)