def callback(data):
global right_arm_pub, left_arm_pub, grippers_indices, left_arm_indices, right_arm_indices
'''print "I hear: ", data
print "Grippers: ", [data.name[i] for i in grippers_indices]
print "Left arm joints: ", [data.name[i] for i in left_arm_indices]
print "Right arm joints: ", [data.name[i] for i in right_arm_indices]'''
if (len(data.name) > 2):
left_arm_msg = JointState()
# left_arm_msg.header.stamp = rospy.Time.now()
left_arm_msg.header = data.header
left_arm_msg.header.frame_id = '/world'
left_arm_msg.name = [data.name[i] for i in left_arm_indices]
left_arm_msg.position = [data.position[i] for i in left_arm_indices]
left_arm_msg.velocity = [data.velocity[i] for i in left_arm_indices]
left_arm_msg.effort = [data.effort[i] for i in left_arm_indices]
left_arm_pub.publish(left_arm_msg)
right_arm_msg = JointState()
# right_arm_msg.header.stamp = rospy.Time.now()
right_arm_msg.header = data.header
right_arm_msg.header.frame_id = '/world'
right_arm_msg.name = [data.name[i] for i in right_arm_indices]
right_arm_msg.position = [data.position[i] for i in right_arm_indices]
right_arm_msg.velocity = [data.velocity[i] for i in right_arm_indices]
right_arm_msg.effort = [data.effort[i] for i in right_arm_indices]
right_arm_pub.publish(right_arm_msg)
def listener(self):
# In ROS, nodes are uniquely named. If two nodes with the same
# node are launched, the previous one is kicked off. The
# anonymous=True flag means that rospy will choose a unique
# name for our 'listener' node so that multiple listeners can
# run simultaneously.
rospy.Subscriber("left_manipulator/joint_states", JointState, self.callback_left_arm)
rospy.Subscriber("right_manipulator/joint_states", JointState , self.callback_right_arm)
rospy.Subscriber("left_gripper/joint_states", JointState, self.callback_left_gripper)
rospy.Subscriber("right_gripper/joint_states", JointState , self.callback_right_gripper)
# spin() simply keeps python from exiting until this node is stopped
joint_state_pub = rospy.Publisher('/joint_states', JointState, queue_size=10)
rate = rospy.Rate(100)
while not rospy.is_shutdown():
output = JointState()
output.name=list()
output.position=list()
output.velocity=list()
output.effort=list()
if self.left_arm_data is not None:
output.header = self.left_arm_data.header
output.name = output.name + self.left_arm_data.name
output.position = output.position + list(self.left_arm_data.position)
output.velocity = output.velocity + list(self.left_arm_data.velocity)
output.effort = output.velocity + list(self.left_arm_data.effort)
if self.right_arm_data is not None:
output.header = self.right_arm_data.header
output.name = output.name + self.right_arm_data.name
rospy.logdebug('output.name = {}'.format(output.name))
output.position = output.position + list(self.right_arm_data.position)
output.velocity = output.velocity + list(self.right_arm_data.velocity)
output.effort = output.velocity + list(self.right_arm_data.effort)
if self.left_gripper_data is not None:
output.header = self.left_gripper_data.header
output.name = output.name + self.left_gripper_data.name
output.position = output.position + list(self.left_gripper_data.position)
output.velocity = output.velocity + list(self.left_gripper_data.velocity)
output.effort = output.velocity + list(self.left_gripper_data.effort)
if self.right_gripper_data is not None:
output.header = self.right_gripper_data.header
output.name = output.name + self.right_gripper_data.name
output.position = output.position + list(self.right_gripper_data.position)
output.velocity = output.velocity + list(self.right_gripper_data.velocity)
output.effort = output.velocity + list(self.right_gripper_data.effort)
joint_state_pub.publish(output)
rospy.logdebug('output = {}'.format(output))
rate.sleep()
def sorted_joint_state_msg(msg, joint_names):
"""
Returns a sorted C{sensor_msgs/JointState} for the given joint names
@type msg: sensor_msgs/JointState
@param msg: The input message
@type joint_names: list
@param joint_names: The sorted joint names
@rtype: sensor_msgs/JointState
@return: The C{JointState} message with the fields in the order given by joint names
"""
valid_names = set(joint_names).intersection(set(msg.name))
valid_position = len(msg.name) == len(msg.position)
valid_velocity = len(msg.name) == len(msg.velocity)
valid_effort = len(msg.name) == len(msg.effort)
num_joints = len(valid_names)
retmsg = JointState()
retmsg.header = copy.deepcopy(msg.header)
for name in joint_names:
if name not in valid_names:
continue
idx = msg.name.index(name)
retmsg.name.append(name)
if valid_position:
retmsg.position.append(msg.position[idx])
if valid_velocity:
retmsg.velocity.append(msg.velocity[idx])
if valid_effort:
retmsg.effort.append(msg.effort[idx])
return retmsg
def talker():
pub = rospy.Publisher('joint_states', JointState, queue_size=20)
rospy.init_node('joint_state_publisher]')
rate = rospy.Rate(10)
left_to_link_1 = -1.9
link_1_to_hidden_link = -1.9
hidden_link_to_link_2 = 0
link_2_to_left_gripper = 0
link_2_to_right_gripper = 0
dt = 0
hello_str = JointState()
hello_str.header = Header()
hello_str.header.stamp = rospy.Time.now()
hello_str.name = [
'left_to_link_1', 'link_1_to_hidden_link', 'hidden_link_to_link_2',
'link_2_to_left_gripper', 'link_2_to_right_gripper'
]
hello_str.position = [
left_to_link_1, link_1_to_hidden_link, hidden_link_to_link_2,
link_2_to_left_gripper, link_2_to_right_gripper
]
hello_str.velocity = [1000, 1000, 1000, 1000, 1000]
hello_str.effort = [0.5, 0.5, 0.5, 0.5, 0.5]
pub.publish(hello_str)
rate.sleep()
while not rospy.is_shutdown():
dt += 0.1
hello_str = modify(hello_str, dt)
pub.publish(hello_str)
rate.sleep()
def sender():
#Defining endEffectorVelocity as global variable to be calculated based on center of ball
global end_eff_vel
end_eff_vel = np.array([0.0,0.0,0.0,0.0,0.0,0.0])
global joint_command
global pre_point
pre_point = Point(0,0,0)
#Creating node, publisher and subscribers
rospy.init_node('velocity_controler', anonymous=True)
jointCommandPublisher = rospy.Publisher('/joint_command', JointState, queue_size=10)
rospy.Subscriber('/ball_center', Point, ballCenterCallback)
rospy.Subscriber('joint_states', JointState, jointStateCallback)
#Publishing Rate
rate = rospy.Rate(100) # in hz
#Defining joint_command
joint_command = JointState()
joint_command.header = std_msgs.msg.Header()
joint_command.header.seq = 0
joint_command.header.stamp = rospy.Time.now()
joint_command.header.frame_id = ''
joint_command.name = ['link_1_joint', 'link_2_joint', 'link_3_joint', 'link_4_joint', 'link_5_joint']
joint_command.position = [0.0, 0.0, 0.0, 0.0, 0.0]
while not rospy.is_shutdown():
#Updating header for joint_command
joint_command.header.seq = joint_command.header.seq + 1
joint_command.header.stamp = rospy.Time.now()
#rospy.loginfo(joint_command)
#Publishing Joint Commands
jointCommandPublisher.publish(joint_command)
rate.sleep()
def homing(q=None, pose=None):
pose = pose or "home"
rospy.init_node('posePublisher', anonymous=True)
pub = rospy.Publisher('pose_state', JointState, queue_size=10)
rate = rospy.Rate(1) # default is 10 Hz
state_str = JointState()
state_str.header = Header()
now = rospy.get_rostime()
state_str.header.stamp.secs = now.secs
state_str.header.stamp.nsecs = now.nsecs
centauro = config.HomePose(pose=pose)
state_str.name = centauro.getName()
if not q:
state_str.position = centauro.getValue()
else:
state_str.position = q
# VARIABLE FOR TERMINATING
ter_publish = False
while not ter_publish:
connections = pub.get_num_connections()
# print "number of connections to this publisher: %s" %connections
if connections > 0:
# rospy.loginfo(state_str) # use for debugging
pub.publish(state_str)
ter_publish = True
# print "Message Published. Closing Program..."
else:
rate.sleep()
def talker():
pub = rospy.Publisher('joint_states', JointState, queue_size=10)
rospy.init_node('joint_state_publisher')
rate = rospy.Rate(10) # 10hz
control_str = JointState()
control_str.header = Header()
control_str.name = [
'joint1', 'joint2', 'joint3', 'joint4', 'joint5', 'joint6'
]
control_str.velocity = []
control_str.effort = []
control_str.position = [0, pi / 2, 0, 0, 0, 0]
control_str.header.stamp = rospy.Time.now()
time.sleep(1.5)
pub.publish(control_str)
rate.sleep()
x0, y0, z0 = [float(i) for i in input("Base position: ").split()]
while not rospy.is_shutdown():
control_str.position = [
float(i) for i in input("Set angle for each joint: ").split()
]
control_str.header.stamp = rospy.Time.now()
pub.publish(control_str)
T = direct([0.5, 1.2, 0.5, 1, 0.1, 1.5])
r = (T * transpose(np.matrix([x0, y0, z0, 1])))[0:3]
print("end effector position: " + str(r))
rate.sleep()
def calculate_normal_trajectory(self, leg_name, subgait_name):
"""Calculate the pose of the non-capture-point leg and set this as the pose for this leg.
:param leg_name: The name of the move group which does not use capture point
:param subgait_name: the normal subgait name
"""
side_prefix = 'right' if 'right' in leg_name else 'left'
default_subgait = deepcopy(self.default_walk[subgait_name])
non_capture_point_joints = []
for joint in default_subgait.joints:
if side_prefix in joint.name:
non_capture_point_joints.append(joint)
joint_state = JointState()
joint_state.header = Header()
joint_state.header.stamp = rospy.Time.now()
joint_state.name = [joint.name for joint in non_capture_point_joints]
joint_state.position = [
joint.setpoints[-1].position for joint in non_capture_point_joints
]
joint_state.velocity = [
joint.setpoints[-1].velocity for joint in non_capture_point_joints
]
self.move_group[leg_name].set_joint_value_target(joint_state)
def __init__(self):
rospy.init_node('manipulator')
rospy.loginfo("Press Ctrl + C to terminate")
self.rate = rospy.Rate(10)
self.joint_pub = rospy.Publisher('/rx150/joint_states',
JointState,
queue_size=10)
# prepare joint message to publish
joint_msg = JointState()
joint_msg.header = Header()
joint_msg.name = ['waist', 'shoulder', 'elbow', 'wrist_angle',\
'wrist_rotate', 'gripper', 'left_finger', 'right_finger']
joint_msg.position = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.026, -0.026]
# test case for forward kinematics
test_case = [pi / 6, -pi / 3, -pi / 6] # joint angle in radian
position = forward_kinematics(test_case)
print(test_case)
print(position)
while not rospy.is_shutdown():
joint_msg.header.stamp = rospy.Time.now()
joint_msg.position[0:3] = test_case
self.joint_pub.publish(joint_msg)
self.rate.sleep()
def new_joint_angles(req):
# left arm is contained in req.position[:]
left_joint_values = 7 * [0.0]
for i, name in enumerate(left_joints):
index = req.left_arm.name.index(name)
left_joint_values[i] = req.left_arm.position[index]
print 'left hand pose:'
print left_joint_values
# call relaxed_ik.solve
xopt = my_relaxedIK.solve(pos_goals, quat_goals, left_joints,
left_joint_values)
print 'solution for right hand:'
print xopt[0:7]
print 'solution for left hand:'
print xopt[7:]
# create response both_arm
both_arms = JointState()
# copy header from req into both_arm
both_arms.header = req.left_arm.header
# both_arm.name = joint_ordering from start_here.py
both_arms.name = joint_ordering
# xopt becomes both_arm.position
both_arms.position = xopt
return NewJointAnglesResponse(both_arms)
def sim_robot():
rospy.init_node("simulated_robot")
pub = rospy.Publisher("joint_states", JointState, queue_size=5)
rospy.Subscriber("joint_1/command", Float64, lambda msg: joint_cb(msg, 0))
rospy.Subscriber("joint_2/command", Float64, lambda msg: joint_cb(msg, 1))
rospy.Subscriber("joint_3/command", Float64, lambda msg: joint_cb(msg, 2))
rospy.Subscriber("joint_4/command", Float64, lambda msg: joint_cb(msg, 3))
rospy.Subscriber("joint_5/command", Float64, lambda msg: joint_cb(msg, 4))
rospy.Subscriber("joint_7/command", Float64, lambda msg: joint_cb(msg, 5))
msg = JointState()
msg.header = Header(stamp=rospy.Time.now())
msg.name = [
"joint_1", "joint_2", "joint_3", "joint_4", "joint_5", "joint_7"
]
msg.position = [
0,
] * 6
msg.velocity = [0, 0, 0, 0, 0, 0]
msg.effort = [0, 0, 0, 0, 0, 0]
def joint_cb(msg_in, id):
msg.header = Header(stamp=rospy.Time.now())
msg.position[id] = msg_in.data
rate = rospy.Rate(10)
while not rospy.is_shutdown():
pub.publish(msg)
rate.sleep()
def talker():
global t
pub = rospy.Publisher('joint_states', JointState, queue_size=10)
rospy.init_node('joint_state_publisher')
rate = rospy.Rate(10) # 10hz
while not rospy.is_shutdown():
hello_str = JointState()
hello_str.header = Header()
hello_str.header.stamp = rospy.Time.now()
hello_str.name = [
'base_to_krylo_front1', 'base_to_krylo_front2',
'base_to_krylo_back1', 'base_to_krylo_back2', 'base_to_antenna',
'base2_to_antenna'
]
hello_str.position = [
math.sin(t), -math.sin(t),
math.sin(t), -math.sin(t),
math.sin(t),
math.sin(t)
]
hello_str.velocity = []
hello_str.effort = []
pub.publish(hello_str)
t += 0.1
rate.sleep()
def gripper_cb(self, data):
js = JointState()
js.header = data.header
js.name = ["Gripper"]
js.position = [data.current_pos]
js.velocity = [data.velocity]
self.gripper_pub.publish(js)
def speechCallback(self, message):
rospy.loginfo("(-O-) Task start (-O-)")
# # initialize
start_state = JointState()
start_state.header = Header()
start_state.header.stamp = rospy.Time.now()
start_state.name = rosparam.get_param("/controller_joint_names")
# start_state.name = rosparam.get_param("/sia5_controller/joints")
for i in range(len(start_state.name)):
start_state.position.append(0.)
# # do the planning
task_N = len(message.goal_pose)
for i in range(task_N):
object_trans = self.get_object_tf_data(message.initial_pose[i])
goal_trans = self.get_goal_tf_data(object_trans, message.goal_pose[i])
if i == 0:
state = self.run(start_state, object_trans, goal_trans)
else:
state = self.run(state, object_trans, goal_trans)
# # print "input key A to continue."
# # while(1):
# # key = raw_input('>>> ')
# # if key == "a":
# # break
if rospy.is_shutdown():
rospy.on_shutdown(self.shutdown)
rospy.loginfo("(^O^) All task finished (^O^)")
def arm_joint_state(self, robot_state=None, fail_if_joint_not_found=True):
'''
Returns the joint state of the arm in the current planning scene state or the passed in state.
**Args:**
*robot_state (arm_navigation_msgs.msg.RobotState):* robot state from which to find the joints.
If None, will use the current robot state in the planning scene interface
*fail_if_joint_not_found (boolean):* Raise a value error if an arm joint is not found in the robot state
**Returns:**
A sensor_msgs.msg.JointState containing just the arm joints as they are in robot_state
**Raises:**
**ValueError:** if fail_if_joint_not_found is True and an arm joint is not found in the robot state
'''
if not robot_state:
robot_state = self._psi.get_robot_state()
joint_state = JointState()
joint_state.header = robot_state.joint_state.header
for n in self.joint_names:
found = False
for i in range(len(robot_state.joint_state.name)):
if robot_state.joint_state.name[i] == n:
joint_state.name.append(n)
joint_state.position.append(
robot_state.joint_state.position[i])
found = True
break
if not found and fail_if_joint_not_found:
raise ValueError('Joint ' + n + ' is missing from robot state')
return joint_state
def homing(q=None, name=None):
rospy.init_node('posePublisher', anonymous=True)
pub = rospy.Publisher('pose_state', JointState, queue_size=10)
rate = rospy.Rate(1) # default is 10 Hz
state_str = JointState()
state_str.header = Header()
now = rospy.get_rostime()
state_str.header.stamp.secs = now.secs
state_str.header.stamp.nsecs = now.nsecs
if not name:
state_str.name = ["J01", "J02"]
else:
state_str.name = name
if not q:
state_str.position = [0, 0]
else:
state_str.position = q
ter_publish = False
while not ter_publish:
connections = pub.get_num_connections()
# print "number of connections to this publisher: %s" %connections
if connections > 0:
# rospy.loginfo(state_str) # use for debugging
pub.publish(state_str)
ter_publish = True
# print "Message Published. Closing Program..."
else:
rate.sleep()
def ik():
pose = Pose2D()
rospy.init_node('ik')
rospy.Subscriber("front_pose", Pose2D, callback, fpose)
rospy.Subscriber("hind_pose", Pose2D, callback, hpose)
pub = rospy.Publisher('joint_states', JointState, queue_size=10)
#rospy.init_node('joint_state_publisher')
pub2 = rospy.Publisher('front_joints', Pose2D, queue_size=3)
rate = rospy.Rate(10) # 10hz
while not rospy.is_shutdown():
#front_joints_str = Pose2D
rospy.loginfo(fpose)
pub2.publish(fpose)
q1, q2, q3, q4 = ikf(fpose.x, fpose.y, fpose.theta)
qf, q5, q6, q7, q8 = ikh(hpose.x, hpose.y, hpose.theta)
joint_str = JointState()
joint_str.header = Header()
joint_str.header.stamp = rospy.Time.now()
joint_str.name = [
'base_to_scapula', 'scapula_to_humerus', 'humerus_to_radius',
'radius_to_carpus', 'base_to_femur', 'femur_to_tibia',
'tibia_to_tarsus', 'tarsus_to_phalange'
]
joint_str.position = [q1, q2, q3, q4, qf + q5, q6, q7, q8]
joint_str.velocity = []
joint_str.effort = []
pub.publish(joint_str)
rate.sleep()
# spin() simply keeps python from exiting until this node is stopped
rospy.spin()
def talker(q):
pub = rospy.Publisher('topic_position_from_invkin',
JointState,
queue_size=10) # This can be seen in rostopic list
rospy.init_node(
'node_position_from_invkin') #This can be seen in rosnode list
hello_str = JointState()
hello_str.header = Header()
hello_str.header.stamp = rospy.Time.now()
hello_str.name = ['prbt_joint_1', 'prbt_joint_2', 'prbt_joint_3']
hello_str.position = []
hello_str.velocity = []
hello_str.effort = []
rate = rospy.Rate(1000) # 10hz
i = 0.0
while not rospy.is_shutdown():
pub.publish(hello_str)
rate.sleep()
hello_str.header.stamp = rospy.Time.now()
hello_str.position = [
i / 1000 * q[0], i / 1000 * q[1], i / 1000 * q[2]
]
i += 1.0
if i == 1000:
break
def callback(data):
#read values from m1 and m2
#rospy.loginfo(rospy.get_caller_id() + "I heard %s", data.data)
try:
motorup_index = data.name.index('gripper_motorup_joint')
motordown_index = data.name.index('gripper_motordown_joint')
except ValueError:
return
if motorup_index >= 0 and motordown_index >= 0:
motorup_pose = data.position[motorup_index]
motordown_pose = data.position[motordown_index]
#calculate finger pose
finger_x = (motorup_pose + motordown_pose) * 0.00124896 / (91 * 4 *
math.pi)
finger_theta = (motorup_pose - motordown_pose) * 0.00124896 / (
91 * 4 * math.pi * 0.048)
#publish to prismatic and revolute joints
msgs_pub = JointState()
msgs_pub.header = Header()
msgs_pub.header.stamp = rospy.Time.now()
msgs_pub.name = [
'gripper_motorup_joint', 'gripper_motordown_joint',
'gripper_prismatic_joint', 'gripper_revolute_joint'
]
msgs_pub.position = [
data.position[motorup_index], data.position[motordown_index],
finger_x, finger_theta
]
msgs_pub.velocity = []
msgs_pub.effort = []
pub.publish(msgs_pub)
def main():
print("INITIALIZING LINK STATE READER")
rospy.init_node("joint_reader")
loop = rospy.Rate(30)
while not rospy.is_shutdown():
model_prop = rospy.ServiceProxy("/gazebo/get_model_properties", GetModelProperties)
model = GetModelPropertiesRequest()
model.model_name = 'justina'
model_prop(model)
joint_names = model_prop(model).joint_names
#print joint_names
joint_prop = rospy.ServiceProxy("/gazebo/get_joint_properties", GetJointProperties)
joint = GetJointPropertiesRequest()
pub_joint = rospy.Publisher('/joint_states', JointState, queue_size=1)
msg_joint = JointState()
for i in joint_names:
joint.joint_name = i
joint_prop(joint)
joint_position = list(joint_prop(joint).position)
joint_position = [float(e) for e in joint_position]
msg_joint.header = Header()
msg_joint.header.stamp = rospy.Time.now()
msg_joint.name = [i]
msg_joint.position = joint_position
msg_joint.velocity = []
msg_joint.effort = []
pub_joint.publish(msg_joint)
#print joint_position
#print i
loop.sleep()
def controlLoop(self):
"""
Runs the control loop
"""
joint_state_gripper = JointState()
joint_state_gripper.header = Header()
joint_state_gripper.header.stamp = rospy.Time.now()
joint_state_gripper.name = [self.joint_name]
joint_state_gripper.position = [self.distance]
joint_state_gripper.velocity = [0.0]
joint_state_gripper.effort = [0.0]
self.gripper_prismatic_joint_state_publisher.publish(
joint_state_gripper)
t_sleep = 1.0 / self.desired_freq
while not rospy.is_shutdown():
joint_state_gripper.header.stamp = rospy.Time.now()
joint_state_gripper.position = [self.distance]
self.gripper_prismatic_joint_state_publisher.publish(
joint_state_gripper)
rospy.sleep(t_sleep)
def get_box_plan(self, num, start_state, grasp):
# Set argument start state
moveit_start_state = RobotState()
moveit_start_state.joint_state = start_state
self.arm.set_start_state(moveit_start_state)
# Calculate goal pose
self.arm.set_joint_value_target(self.box_pose[num])
# plan
plan = RobotTrajectory()
counter = 0
while len(plan.joint_trajectory.points) == 0:
plan = self.arm.plan()
counter += 1
self.arm.set_planning_time(self.planning_limitation_time +
counter * 5.0)
if counter > 1:
return (False, start_state)
self.arm.set_planning_time(self.planning_limitation_time)
rospy.loginfo("!! Got a box plan !!")
# publish the plan
pub_msg = HandringPlan()
pub_msg.grasp = grasp
pub_msg.trajectory = plan
self.hp_pub.publish(pub_msg)
self.arm.clear_pose_targets()
# return goal state from generated trajectory
goal_state = JointState()
goal_state.header = Header()
goal_state.header.stamp = rospy.Time.now()
goal_state.name = plan.joint_trajectory.joint_names[:]
goal_state.position = plan.joint_trajectory.points[-1].positions[:]
return (True, goal_state, plan)
def sorted_joint_state_msg(msg, joint_names):
"""
Returns a sorted C{sensor_msgs/JointState} for the given joint names
@type msg: sensor_msgs/JointState
@param msg: The input message
@type joint_names: list
@param joint_names: The sorted joint names
@rtype: sensor_msgs/JointState
@return: The C{JointState} message with the fields in the order given by joint names
"""
valid_names = set(joint_names).intersection(set(msg.name))
valid_position = len(msg.name) == len(msg.position)
valid_velocity = len(msg.name) == len(msg.velocity)
valid_effort = len(msg.name) == len(msg.effort)
num_joints = len(valid_names)
retmsg = JointState()
retmsg.header = copy.deepcopy(msg.header)
for name in joint_names:
if name not in valid_names:
continue
idx = msg.name.index(name)
retmsg.name.append(name)
if valid_position:
retmsg.position.append(msg.position[idx])
if valid_velocity:
retmsg.velocity.append(msg.velocity[idx])
if valid_effort:
retmsg.effort.append(msg.effort[idx])
return retmsg
def default(self, ci="unused"):
""" Publish the data of the posture sensor as a ROS JointState message """
js = JointState()
js.header = self.get_ros_header()
js.name = [
"kuka_arm_0_joint",
"kuka_arm_1_joint",
"kuka_arm_2_joint",
"kuka_arm_3_joint",
"kuka_arm_4_joint",
"kuka_arm_5_joint",
"kuka_arm_6_joint",
"head_pan_joint",
"head_tilt_joint",
]
js.position = [
self.data["seg0"],
self.data["seg1"],
self.data["seg2"],
self.data["seg3"],
self.data["seg4"],
self.data["seg5"],
self.data["seg6"],
self.data["pan"],
self.data["tilt"],
]
# js.velocity = [1, 1, 1, 1, 1, 1, 1]
# js.effort = [50, 50, 50, 50, 50, 50, 50]
self.publish(js)
def arm_joint_state(self, robot_state=None, fail_if_joint_not_found=True):
'''
Returns the joint state of the arm in the current planning scene state or the passed in state.
**Args:**
*robot_state (arm_navigation_msgs.msg.RobotState):* robot state from which to find the joints.
If None, will use the current robot state in the planning scene interface
*fail_if_joint_not_found (boolean):* Raise a value error if an arm joint is not found in the robot state
**Returns:**
A sensor_msgs.msg.JointState containing just the arm joints as they are in robot_state
**Raises:**
**ValueError:** if fail_if_joint_not_found is True and an arm joint is not found in the robot state
'''
if not robot_state:
robot_state = self._psi.get_robot_state()
joint_state = JointState()
joint_state.header = robot_state.joint_state.header
for n in self.joint_names:
found = False
for i in range(len(robot_state.joint_state.name)):
if robot_state.joint_state.name[i] == n:
joint_state.name.append(n)
joint_state.position.append(robot_state.joint_state.position[i])
found = True
break
if not found and fail_if_joint_not_found:
raise ValueError('Joint '+n+' is missing from robot state')
return joint_state
def talker():
pub_joints = rospy.Publisher('joint_states', JointState, queue_size=10)
rospy.init_node('joint_state_publisher')
rate = rospy.Rate(10) # 10hz
joint_states = JointState()
joint_states.header = Header()
joint_states.header.stamp.secs = rospy.get_time()
joint_states.name = ["joint_1",
"joint_2",
"joint_3",
"joint_4",
"joint_5",
"joint_6",
"joint_7",
"qbhand_synergy_joint"]
joint_states.position = [0.0,
0.0,
0.0,
pi/2,
0.0,
pi/2,
0.0,
0.3]
joint_states.velocity = []
joint_states.effort = []
rospy.loginfo(joint_states.header.stamp)
while not rospy.is_shutdown():
joint_states.header.stamp.secs = rospy.get_time()
pub_joints.publish(joint_states)
rate.sleep()
def move(self, x, y, z, orientation_matrix=np.eye(3)):
joint_states = self.chain.inverse_kinematics(
geometry_utils.to_transformation_matrix([x, y, z],
orientation_matrix))
# joint_states = self.chain.inverse_kinematics([ [-.5, 0, 0, x],
# [0, 0.5, 0, y],
# [0, 0, 0.5, z],
# [0, 0, 0, -.5] ])
print(joint_states)
hdr = Header()
hdr.seq = self.seq = self.seq + 1
hdr.stamp = rospy.Time.now()
hdr.frame_id = "My-Kinematic-Thingy"
js = JointState()
js.header = hdr
js.name = [
"Joint_1", "Joint_2", "Joint_3", "Joint_4", "Joint_5", "Joint_6",
"Joint_Gripper"
] #, ,
# "Joint_Grip_Servo", "Joint_Tip_Servo", "Joint_Grip_Servo_Arm",
# "Joint_Grip_Idle", "Joint_Tip_Idle", "Joint_Grip_Idle_Arm"]
js.position = joint_states[1:8]
js.velocity = []
js.effort = []
self.publisher.publish(js)
def armMain():
global posAct
global actual_J, pos
global t, st, et
#global pos0,pos1,pos2,pos3,pos4,pos5
#actual_J = posAct
rospy.init_node('sim_arm')
rospy.Subscriber('joint_states', JointState, joints_Callback)
pub = rospy.Publisher('joint_states', JointState, queue_size=10)
rate = rospy.Rate(10) # 10hz
st = time.time()
#plt.plot(t,pos)
#plt.subplot(611)
#ax1 = plt.subplot(6,1,1)
plt.show()
plt.clf()
while not rospy.is_shutdown():
v = agregarRuido()
hello_str = JointState()
hello_str.header = Header()
hello_str.header.stamp = rospy.Time.now()
hello_str.name = ['robocol_joint1','robocol_joint2',\
'robocol_joint3','robocol_joint4','robocol_joint5','robocol_joint6']
hello_str.position = v
#hello_str.position = [actual_J0,actual_J1,actual_J2,\
#actual_J3,j4,actual_J5]
hello_str.velocity = [0, 0, 0, 0, 0, 0]
hello_str.effort = [0, 0, 0, 0, 0, 0]
hello_str.header.stamp = rospy.Time.now()
#pub.publish(hello_str)
et = round(time.time() - st, 3)
t.append(et)
getPos()
plotPos()
#plt.plot([1,2],[1,2],color='b')
#plt.show()
#getVel()
#plotVel()
#pos0.append(actual_J[0])
#pos1.append(actual_J[1])
#pos2.append(actual_J[2])
#pos3.append(actual_J[3])
#pos4.append(actual_J[4])
#pos5.append(actual_J[5])
# plt.subplot(611)
# plt.scatter(t,pos0,color='b')
# plt.subplot(612)
# plt.scatter(t,pos1,color='b')
# plt.subplot(613)
# plt.scatter(t,pos2,color='b')
# plt.subplot(614)
# plt.scatter(t,pos3,color='b')
# plt.subplot(615)
# plt.scatter(t,pos4,color='b')
# plt.subplot(616)
# plt.scatter(t,pos5,color='b')
# plt.pause(0.05)
rate.sleep()
def _broadcastJointState(self, joint_state_publisher, extra_joints=None):
"""
INTERNAL METHOD, publishes the state of the robot's joints into the ROS
framework
Parameters:
joint_state_publisher - The ROS publisher for the JointState
message, describing the state of the robot's joints
extra_joints - A dict, describing extra joints to be published. The
dict should respect the following syntax:
{"joint_name": joint_value, ...}
"""
msg_joint_state = JointState()
msg_joint_state.header = Header()
msg_joint_state.header.stamp = rospy.get_rostime()
msg_joint_state.name = list(self.robot.joint_dict)
msg_joint_state.position = self.robot.getAnglesPosition(
msg_joint_state.name)
try:
assert isinstance(extra_joints, dict)
for name, value in extra_joints.items():
msg_joint_state.name += [name]
msg_joint_state.position += [value]
except AssertionError:
pass
joint_state_publisher.publish(msg_joint_state)
def cmd_callback(self, msg):
ang = self.ik(msg)
if self.current_bad:
rospy.loginfo("Current Spike! Stopping Movements...")
msg2 = rospy.wait_for_message('pot', Float32MultiArray)
self.pot_pub.publish(msg2)
msg3 = self.pot_to_angle(msg2)
self.ang_pub.publish(msg3)
elif ang == -1:
rospy.loginfo("Invalid pose!")
else:
rospy.loginfo("Good pose! Publishing..." + str(ang.data[0]) + " " +
str(ang.data[1]) + " " + str(ang.data[2]))
self.ang_pub.publish(ang)
pot = self.angle_to_pot(ang)
self.pot_pub.publish(ang)
h = Header()
h.stamp = rospy.Time.now()
joint_state = JointState()
joint_state.header = h
joint_state.name = [
"base_to_arm_1", "arm_1_to_arm_2", "arm_2_to_arm_3",
"left_front_wheel_joint", "left_back_wheel_joint",
"right_front_wheel_joint", "right_back_wheel_joint"
]
joint_state.position = ang.data + [0, 0, 0, 0]
self.joint_pub.publish(joint_state)
def pubJoint(armDataL, armDataR):
pub = rospy.Publisher('joint_states', JointState, queue_size=10)
rospy.init_node('joint_state_publisher')
rate = rospy.Rate(10) # 10hz
wholeJoints = JointState()
wholeJoints.header = Header()
wholeJoints.name = ['torso_joint_b1', 'torso_joint_b2', 'arm_left_joint_1_s', 'arm_left_joint_2_l', 'arm_left_joint_3_e',
'arm_left_joint_4_u', 'arm_left_joint_5_r', 'arm_left_joint_6_b', 'arm_left_joint_7_t',
'arm_right_joint_1_s', 'arm_right_joint_2_l', 'arm_right_joint_3_e', 'arm_right_joint_4_u',
'arm_right_joint_5_r', 'arm_right_joint_6_b', 'arm_right_joint_7_t']
wholeJoints.velocity = []
wholeJoints.effort = []
tTotal = np.size(armDataL,0)
qArmL = np.zeros((7,), dtype=float)
qArmR = np.zeros((7,), dtype=float)
qTorso2 = [0.0, 0.0]
while not rospy.is_shutdown():
for i in range(tTotal):
qArmL = armDataL[i,:]
qArmR = armDataR[i,:]
qArmR7 = qArmR.tolist()
qArmL7 = qArmL.tolist()
wholeJoints.position = qTorso2 + qArmL7 + qArmR7
wholeJoints.header.stamp = rospy.Time.now()
pub.publish(wholeJoints)
rate.sleep()
def transform_callback(data):
px = data.pose.position.x
py = data.pose.position.y
pz = data.pose.position.z
r = quaternion_matrix([data.pose.orientation.x, data.pose.orientation.y, data.pose.orientation.z, data.pose.orientation.w])
a = 0.5
th1 = atan2(py, px)
th2 = asin(-pz/a)
th3 = asin(-r[2][0])-th2
th3_2 = acos(r[0][0]*cos(th1) + r[1][0]*sin(th1)) - th2
if abs(th3_2) > abs(th3):
th3 = th3_2
print(str(th3)+str(th3_2))
#if req.time <= 0 or not 0 <= req.j1 <= 6 or not -3 <= req.j2 <= 3 or not -1 <= req.j3 <= 1:
# return False
hello_str = JointState()
hello_str.header = Header()
hello_str.header.stamp = rospy.Time.now()
hello_str.name = ['1_na_boki', '1_przod_tyl', 'czlon_z_2']
hello_str.position = [th_0[0]+th1, th_0[1]+th2, th_0[2]+th3]
hello_str.velocity = []
hello_str.effort = []
pub.publish(hello_str)
current_time = 0
return (str(th1)+" "+str(th2)+" "+str(th3))
def sim_robot():
rospy.init_node("simulated_robot")
pub = rospy.Publisher("joint_states", JointState, queue_size=5)
rospy.Subscriber("joint_1/command", Float64, lambda msg: joint_cb(msg, 0))
rospy.Subscriber("joint_2/command", Float64, lambda msg: joint_cb(msg, 1))
rospy.Subscriber("joint_3/command", Float64, lambda msg: joint_cb(msg, 2))
rospy.Subscriber("joint_4/command", Float64, lambda msg: joint_cb(msg, 3))
rospy.Subscriber("joint_5/command", Float64, lambda msg: joint_cb(msg, 4))
rospy.Subscriber("joint_7/command", Float64, lambda msg: joint_cb(msg, 5))
msg = JointState()
msg.header = Header(stamp=rospy.Time.now())
msg.name = ["joint_1", "joint_2", "joint_3", "joint_4", "joint_5", "joint_7"]
msg.position = [0] * 6
msg.velocity = [0, 0, 0, 0, 0, 0]
msg.effort = [0, 0, 0, 0, 0, 0]
def joint_cb(msg_in, id):
msg.header = Header(stamp=rospy.Time.now())
msg.position[id] = msg_in.data
rate = rospy.Rate(10)
while not rospy.is_shutdown():
pub.publish(msg)
rate.sleep()
def move_all_joints(self, position_array):
rospy.logwarn("move_all_joints STARTED")
# We check that the position array has the correct number of elements
number_of_joints = len(self.joint_states_msg.name)
if len(position_array) == number_of_joints:
if self.check_gripper_pos_safe(position_array[6]):
new_joint_position = JointState()
h = Header()
h.stamp = rospy.Time.now() # Note you need to call rospy.init_node() before this will work
h.frame_id = self.joint_states_msg.header.frame_id
new_joint_position.header = h
new_joint_position.name = self.joint_states_msg.name
new_joint_position.position = position_array
# These values arent used, so they dont matter really
new_joint_position.velocity = self.joint_states_msg.velocity
new_joint_position.effort = self.joint_states_msg.effort
rospy.logwarn("PUBLISH STARTED")
self.goal_dynamixel_position_publisher.publish(new_joint_position)
rospy.logwarn("PUBLISH FINISHED")
else:
rospy.logerr("Gripper position NOT valid=" + str(position_array[6]))
else:
rospy.logerr("The Array given doesnt have the correct length="+str(number_of_joints))
rospy.logwarn("move_all_joints FINISHED")
def interpol(jReq): if(jReq.j1>1 or jReq.j2>1 or jReq.j3>1): return False global start curr_time = 0. end = [jReq.j1, jReq.j2, jReq.j3] change = start for k in range(0, int(freq*jReq.time)+1): for i in range(0, 3): change[i]=inter(change[i],end[i],jReq.time,curr_time,jReq.iflin) pose_str = JointState() pose_str.header = Header() pose_str.header.stamp = rospy.Time.now() pose_str.name = ['i_1', 'i_2', 'i_3'] pose_str.position = [change[0], change[1], change[2]] publisher.publish(pose_str) curr_time = curr_time +1./freq rate.sleep() start = end return True
def main(ra):
# Create Pose Publisher and Subscriber
joint_pub= rospy.Publisher('/dvrk/PSM1/set_torque_joint', JointState, queue_size=10)
rospy.Subscriber('/dvrk/PSM1/state_joint_current', JointState, callback_joint)
rospy.init_node('Talkity_talk_talk',anonymous=True)
rate = rospy.Rate(ra) # 1hz
#hardcode home to zero
joint_msg= JointState()
joint_msg.header = Header()
joint_msg.header.stamp = rospy.Time.now()
#joint_msg.name = ['outer_yaw','o', 'outer_pitch', 'outer_insertion', 'outer_roll', 'outer__wrist_pitch','outer_wrist_jaw', 'jaw']
joint_msg.name = ['outer_yaw', 'outer_pitch', 'outer_insertion', 'outer_roll', 'outer__wrist_pitch','outer_wrist_jaw', 'jaw']
joint_msg.position=[]
joint_msg.velocity = []
joint_msg.effort = []
#Excitation Trajectory Position Coordinates
i=0
q_data1=[-0.2, -0.2, -0.2, -0.2]
q_data2=[-0.086355,-0.095648,-0.10506,-0.1145,-0.12387,-0.13311,-0.14214,-0.15087,-0.15925,-0.16721,-0.17471,-0.18172,-0.18821,-0.19416,-0.19958,-0.20446,-0.20883,-0.21268,-0.21606,-0.21898,-0.22147,-0.22357,-0.2253,-0.22669,-0.22779,-0.22861,-0.22919,-0.22954,-0.2297,-0.22968,-0.2295,-0.22914,-0.22862,-0.22795,-0.22713,-0.22617,-0.22511,-0.22388,-0.22247,-0.22082,-0.21888,-0.21662,-0.214,-0.21095,-0.20744,-0.20342,-0.19885,-0.19368,-0.18788,-0.1814,-0.17424,-0.16635,-0.15775,-0.14843,-0.13843,-0.12778,-0.11651,-0.10471,-0.092438,-0.079795,-0.066886,-0.053804,-0.040655,-0.027542,-0.014584,-0.0018928,0.010422,0.022271,0.033597,0.044314,0.05438,0.063868,0.072648,0.080705,0.088029,0.09461,0.10049,0.10573,0.11037,0.11443,0.11795,0.12097,0.12353,0.12566,0.12736,0.12866,0.12956,0.13007,0.13023,0.13005,0.12955,0.12874,0.12764,0.12629,0.12472,0.12297,0.12106,0.11903,0.11691,0.11471,0.11247,0.11021,0.10793,0.10564,0.10335,0.10109,0.098863,0.096666,0.094485,0.092317,0.09015,0.087973,0.085786,0.083574,0.081318,0.079023,0.076691,0.074343,0.071992,0.069651,0.067341,0.065096,0.06294,0.060911,0.05905,0.057395,0.055962,0.05478,0.053876,0.053265,0.052956,0.052948,0.053206,0.053699,0.054398,0.055243,0.056172,0.057127,0.058038,0.05881,0.059392,0.059703,0.05959,0.059125,0.058272,0.057011,0.055343,0.053232,0.050668,0.047655,0.044237,0.040451,0.036343,0.031955,0.027341,0.022586,0.017764,0.012952,0.0082075,0.0035806 -0.00086291,-0.0050626,-0.0089787,-0.012584,-0.015859,-0.018781,-0.021356,-0.023586,-0.025488,-0.027074,-0.028361,-0.029398,-0.030214,-0.030849,-0.031337,-0.031706,-0.031991,-0.032234,-0.032468,-0.032704,-0.032954,-0.033233,-0.033555,-0.033912,-0.034301,-0.034697,-0.035081,-0.035422,-0.035697,-0.035867,-0.035877,-0.035676,-0.035207,-0.03442,-0.033253,-0.031662,-0.029576,-0.026944,-0.023725,-0.019887,-0.015407,-0.010266,-0.0044619,0.0020043,0.0091073,0.016811,0.025072,0.033839,0.04304,0.052649,0.062524,0.072569,0.082709,0.092857,0.10293,0.11281,0.12243,0.13173,0.14065,0.14913,0.15713,0.16459,0.1715,0.17786,0.18366,0.1889,0.19362,0.19783,0.20158,0.20492,0.2079,0.21056,0.21295,0.21512,0.21715,0.21904,0.22085,0.22259,0.2243,0.22599,0.22766,0.22932,0.23095,0.23255,0.23409,0.23556,0.23692,0.23812,0.23907,0.2398,0.24025,0.24034,0.24004,0.23926,0.23797,0.23611,0.23359,0.23033,0.22627,0.22136,0.21557,0.20886,0.20118,0.19255,0.18296,0.17243,0.16101,0.14864,0.13538,0.12151,0.10701,0.091903,0.07631,0.060357,0.044198,0.02793,0.011667,-0.0044587,-0.020288,-0.035661,-0.050484,-0.064586,-0.07786,-0.090235,-0.10163,-0.11199,-0.12125,-0.12941,-0.13646,-0.14241,-0.14728,-0.15105,-0.15373,-0.15539,-0.15605,-0.15578,-0.15459,-0.15257,-0.14976,-0.14623,-0.1421,-0.13743,-0.1323,-0.12678,-0.12094,-0.11485,-0.10862,-0.10231,-0.095985,-0.089689,-0.083481,-0.077412,-0.071513,-0.065797,-0.060263,-0.054899,-0.04968,-0.044575,-0.039542,-0.034456,-0.029335,-0.024135,-0.018785,-0.013241,-0.0074516,-0.0014066,0.0049231,0.011561,0.018514,0.025767,0.033291,0.041039,0.048948,0.056956,0.06499,0.072953,0.080753,0.088337,0.095636,0.10252,0.10875,0.11438,0.11943,0.12382,0.12749,0.13041,0.13264,0.1342,0.13509,0.13531,0.13486,0.13379,0.13214,0.12997,0.12732,0.12423,0.12075,0.11694,0.11286,0.10857,0.1041,0.099486,0.09475,0.08992,0.085004,0.080014,0.074935,0.069724,0.064326,0.058826,0.053265,0.047541,0.04157,0.035322,0.028833,0.02214,0.015199,0.0079713,0.00039706,-0.007471,-0.015506,-0.023615,-0.031787,-0.039995,-0.048156,-0.056162,-0.063921,-0.071375,-0.078458,-0.0851,-0.091259,-0.096872,-0.10188,-0.10625,-0.10998,-0.1131,-0.11564,-0.11763,-0.11913,-0.1202,-0.12096,-0.12153,-0.122,-0.12247,-0.12303,-0.12377,-0.12477,-0.12608,-0.1278,-0.1299,-0.13235,-0.13516,-0.13832,-0.14178,-0.14548,-0.14934,-0.15332,-0.15736,-0.16139,-0.16534,-0.16914,-0.17275,-0.17613,-0.1793,-0.18218,-0.18474,-0.18697,-0.18885,-0.19036,-0.19152,-0.19237,-0.19292,-0.19317,-0.19312,-0.19282,-0.19229,-0.19154,-0.19058,-0.18941,-0.18806,-0.18664,-0.18523,-0.18373,-0.18213,-0.18048,-0.17888,-0.17739,-0.17596,-0.17458,-0.17322,-0.17194,-0.17084,-0.16996,-0.16924,-0.16864,-0.16818,-0.16787,-0.16771,-0.16764,-0.16761,-0.1676,-0.16757,-0.1675,-0.16736,-0.16713,-0.16679,-0.1664,-0.16587,-0.16524,-0.16451,-0.1637,-0.16282,-0.16188,-0.16092,-0.15998,-0.15907,-0.15823,-0.15746,-0.15675,-0.1561,-0.15549,-0.15491,-0.15431,-0.15365,-0.15287,-0.15191,-0.15069,-0.14918,-0.14729,-0.14498,-0.14218,-0.13885,-0.13497,-0.13052,-0.12547,-0.11987,-0.11377,-0.10721,-0.10028,-0.093075,-0.085649]
q_data3=[-0.079852,-0.082139,-0.083106,-0.082646,-0.080594,-0.076762,-0.071134,-0.063814,-0.054815,-0.044135,-0.031832,-0.01809,-0.0031553,0.012866,0.029824,0.047487,0.065601,0.083926,0.10234,0.12065,0.1387,0.15626,0.17319,0.18942,0.20498,0.21978,0.23366,0.24665,0.2588,0.27022,0.28092,0.29103,0.30017,0.30862,0.31667,0.32424,0.33127,0.33778,0.34374,0.34927,0.35453,0.35941,0.36388,0.368,0.37184,0.37538,0.37857,0.38148,0.38422,0.38678,0.38903,0.39112,0.39306,0.39486,0.39652,0.39814,0.39971,0.40112,0.40243,0.40374,0.40498,0.40614,0.40727,0.40825,0.40922,0.41017,0.41094,0.41154,0.41218,0.41281,0.41336,0.4141,0.41439,0.4145,0.41497,0.41573,0.41643,0.41699,0.41767,0.41851,0.41953,0.42069,0.42196,0.42346,0.42504,0.42669,0.42843,0.43035,0.43243,0.43441,0.43623,0.43786,0.43943,0.44093,0.44214,0.44285,0.44304,0.44289,0.4424,0.44132,0.43955,0.43711,0.43405,0.4307,0.42677,0.4221,0.41688,0.41125,0.40524,0.39895,0.39239,0.38549,0.37853,0.37166,0.3646,0.35757,0.35082,0.34449,0.33832,0.33227,0.32652,0.32142,0.31679,0.31253,0.30862,0.30518,0.30219,0.29973,0.29775,0.2961,0.29478,0.29393,0.29349,0.29333,0.29348,0.29395,0.29472,0.29585,0.29723,0.29875,0.30057,0.30268,0.30472,0.30738,0.31034,0.31319,0.31598,0.31898,0.32227,0.32572,0.32922,0.33272,0.33621,0.33977,0.34334,0.34694,0.35048,0.35394,0.35726,0.36057,0.36358,0.36645,0.36937,0.37217,0.37462,0.37681,0.3788,0.38067,0.3824,0.38385,0.38505,0.386,0.38683,0.3876,0.3882,0.38855,0.38878,0.38903,0.38924,0.3894,0.38954,0.38964,0.38973,0.38987,0.38996,0.39024,0.39045,0.39056,0.39054,0.39057,0.39061,0.39054,0.39022,0.38975,0.38923,0.38866,0.38787,0.3868,0.38548,0.38407,0.38257,0.38089,0.37898,0.37685,0.37461,0.37221,0.36953,0.3665,0.36309,0.3591,0.35474,0.3498,0.34393,0.33692,0.32875,0.31955,0.30912,0.29724,0.28371,0.26859,0.25196,0.23391,0.21438,0.19339,0.17107,0.14772,0.12357,0.098775,0.073515,0.04809,0.022717,-0.0019257,-0.025823,-0.04901,-0.071158,-0.091765,-0.11066,-0.12781,-0.14327,-0.157,-0.16883,-0.17863,-0.18654,-0.19287,-0.19781,-0.20126,-0.20332,-0.20431,-0.2044,-0.20379,-0.20265,-0.20106,-0.19905,-0.19676,-0.19444,-0.19206,-0.18958,-0.18694,-0.18415,-0.18108,-0.17764,-0.17373,-0.16921,-0.16387,-0.15758,-0.15023,-0.1417,-0.13161,-0.11999,-0.10746,-0.093137,-0.076579,-0.05832,-0.038796,-0.018042,0.0040629,0.027463,0.051969,0.077223,0.10292,0.12895,0.15479,0.18029,0.20545,0.23013,0.25388,0.27614,0.29682,0.31604,0.33383,0.34993,0.36403,0.37608,0.38615,0.39409,0.40046,0.40509,0.4075,0.40754,0.40552,0.40177,0.39601,0.38801,0.37773,0.36534,0.35088,0.33431,0.31538,0.29427,0.27131,0.24652,0.2198,0.19133,0.16153,0.13065,0.098926,0.066691,0.034325,0.0020559,-0.029857,-0.060853,-0.090528,-0.11874,-0.14525,-0.16976,-0.19203,-0.21181,-0.22901,-0.24362,-0.25566,-0.26511,-0.27201,-0.27647,-0.2787,-0.27896,-0.27737,-0.27409,-0.26965,-0.26386,-0.25687,-0.24871,-0.2406,-0.23276,-0.22456,-0.21582,-0.20693,-0.19843,-0.19045,-0.18292,-0.17577,-0.16904,-0.16281,-0.15715,-0.15211,-0.14776,-0.14411,-0.14096,-0.13819,-0.13587,-0.13402,-0.13255,-0.13125,-0.12989,-0.12842,-0.12654,-0.1239,-0.12097,-0.11708,-0.11161,-0.10524,-0.098217,-0.089336,-0.077418,-0.063179,-0.04798,-0.031811,-0.01369,0.0068895,0.029129,0.052422,0.076567,0.1018,0.12787,0.15402,0.17991,0.20572,0.23111,0.25527,0.27779,0.29889,0.3186,0.33655,0.35244,0.36616,0.37776,0.3874,0.39513,0.40098,0.40494,0.40711,0.40757,0.40651,0.40409,0.40054,0.39578,0.38979,0.38254,0.37412,0.36496,0.35435,0.34238,0.32931,0.3152,0.29976,0.28285,0.26448,0.24487,0.22398,0.20186,0.17857,0.15437,0.12942,0.10371,0.076993,0.050524,0.024549,-0.0014408,-0.027645,-0.05315,-0.077226,-0.10005,-0.12186,-0.14242,-0.16106,-0.17779,-0.19311,-0.20692,-0.21881,-0.22894,-0.23753,-0.24408,-0.24874,-0.25271,-0.25664,-0.2598,-0.26109,-0.26095,-0.26058,-0.26071,-0.26048,-0.25967,-0.25854,-0.2577,-0.25711,-0.25636,-0.2554,-0.25472,-0.25436,-0.25387,-0.25307,-0.25219,-0.2515,-0.25087,-0.25005,-0.24893,-0.24764,-0.24649,-0.24529,-0.24406,-0.24276,-0.24139,-0.23999,-0.2387,-0.23762,-0.23689,-0.23632,-0.23591,-0.23565,-0.23566,-0.23601,-0.23656,-0.23719,-0.23776,-0.2382,-0.23847,-0.23848,-0.23807,-0.23715,-0.2354,-0.2328,-0.22933,-0.225,-0.21972,-0.21329,-0.20536,-0.19699,-0.18807,-0.17793,-0.16645,-0.15449,-0.14255]
q_output=[None]*len(q_data1)
qd_output=[None]*len(q_data1)
torque_output=[None]*len(q_data1)
while not rospy.is_shutdown():
if i>len(q_data1)-1:
i=0;
joint_msg.position = []
joint_msg.velocity = []
joint_msg.effort=[q_data1[i],0,0,0,0,0,0]
#joint_msg.effort = [joint_sub.effort[0],joint_sub.effort[1],joint_sub.effort[2],joint_sub.effort[3],joint_sub.effort[4],joint_sub.effort[5],joint_sub.effort[6],joint_sub.effort[7]]
print(joint_msg)
joint_pub.publish(joint_msg)
q_output[i]=joint_sub.position
qd_output[i]=joint_sub.velocity
torque_output[i]=joint_sub.effort
rospy.sleep(1/float(ra))
i+=1
rate.sleep()
#Print Code
with open('data_position.csv', 'wb') as myfile:
wr = csv.writer(myfile, quoting=csv.QUOTE_NONE)
for i in range(0,len(q_data1)):
wr.writerow(q_output[i])
with open('data_velocity.csv', 'wb') as myfile:
wr = csv.writer(myfile, quoting=csv.QUOTE_NONE)
for i in range(0,len(q_data1)):
wr.writerow(qd_output[i])
with open('data_torque.csv', 'wb') as myfile:
wr = csv.writer(myfile, quoting=csv.QUOTE_NONE)
for i in range(0,len(q_data1)):
wr.writerow(torque_output[i])
def _get_neutral_joint_state(self, njs):
js = JointState()
js.header = Header(stamp=rospy.Time.now(), frame_id='base')
for j in njs:
js.name.append(j)
js.position.append(njs[j])
return js
def default(self, ci='unused'):
js = JointState()
js.header = self.get_ros_header()
joints = fill_missing_pr2_joints(self.data)
js.name = joints.keys()
js.position = joints.values()
self.publish(js)
def talker():
global joy_value
global last_joy
global joint_state
rospy.init_node('vsv_arm_teleop_raw')
sub = rospy.Subscriber('~joy', Joy, joy_cb)
jsub = rospy.Subscriber('~joint_state', JointState, joint_cb)
joint = rospy.Publisher('~joint_command', JointState)
axis={}
axis["ArmPan"] = [int(s) for s in str(rospy.get_param("~axis_pan","6")).split(",")]
axis["ArmTilt"] = [int(s) for s in str(rospy.get_param("~axis_tilt","7")).split(",")]
axis["ArmFold"] = [int(s) for s in str(rospy.get_param("~axis_fold","4,5")).split(",")]
axis["ArmExtend"] = [int(s) for s in str(rospy.get_param("~axis_extend","0,3")).split(",")]
axis["ToolRotate"] = [int(s) for s in str(rospy.get_param("~axis_rotate","2,1")).split(",")]
step_value = {"ArmPan":1*pi/180., "ArmTilt":5*pi/180.,
"ArmFold":2*pi/180., "ArmExtend":0.05, "ToolRotate":5*pi/180.}
timeout = True
rate = rospy.Rate(10)
while not rospy.is_shutdown() and not joint_state:
rate.sleep()
if rospy.is_shutdown():
return
command = JointState()
command.header = joint_state.header
command.name = joint_state.name
command.velocity = [0.0] * len(joint_state.name)
command.effort = [0.0] * len(joint_state.name)
command.position = [x for x in joint_state.position]
index = dict(zip(command.name,range(0,len(command.name))))
while not rospy.is_shutdown():
if (rospy.rostime.get_time() - last_joy) < 0.5:
if timeout:
timeout = False
rospy.loginfo("Accepting joystick commands")
for k in axis.keys():
if len(axis[k])==1:
# Axis
command.position[index[k]] = sat(command.position[index[k]]+joy_value.axes[axis[k][0]]*step_value[k],
joint_state.position[index[k]],step_value[k])
else:
# Buttons
if joy_value.buttons[axis[k][0]]:
command.position[index[k]] = sat(command.position[index[k]]-step_value[k],
joint_state.position[index[k]],step_value[k])
elif joy_value.buttons[axis[k][1]]:
command.position[index[k]] = sat(command.position[index[k]]+step_value[k],
joint_state.position[index[k]],step_value[k])
# if twist.linear.x < 0:
# twist.angular.z = - twist.angular.z
else:
if not timeout:
timeout = True
rospy.loginfo("Timeout: ignoring joystick commands")
joint.publish(command)
rate.sleep()
def receieveJointStates(self, msg):
if (time.time() - self.lastJointSaveTime > self.streamSavingPeriod):
armState = JointState()
armState.header = msg.header
for i in range(len(msg.name)):
if (msg.name[i] in self.allJoints):
armState.name.append(msg.name[i])
armState.position.append(msg.position[i])
self.jointStateBag.write('arm_states', armState)
self.lastJointSaveTime = time.time()
def default(self, ci='unused'):
""" Publish the data of the posture sensor as a ROS JointState message """
js = JointState()
js.header = self.get_ros_header()
joints = fill_missing_pr2_joints(self.data)
js.name = joints.keys()
js.position = joints.values()
self.publish(js)
def convertJointTrajectorySensorMsg(self, msg):
# Generate fake JointState msg
sim_data_msg = JointState()
sim_data_msg.header = msg.header
sim_data_msg.name = msg.joint_names
sim_data_msg.position = msg.actual.positions
sim_data_msg.velocity = msg.actual.velocities
sim_data_msg.effort = msg.actual.effort
return sim_data_msg
def sync_joint_cb(self,*args):
self.connected = True
js = JointState()
js.header = args[0].header
for j in args:
js.name+=j.name
js.position+=j.position
js.velocity+=j.velocity
js.effort+=j.effort
self.joint_pub.publish(js)
def default(self, ci='unused'):
js = JointState()
js.header = self.get_ros_header()
js.name = ['head_pan_joint', 'head_tilt_joint']
js.position = [
self.data['pan'],
self.data['tilt']
]
self.publish(js)
def odomcb(self, data):
self.X = self.odom_to_state(data)
# if self.running_flag:
joint_states= JointState()
joint_states.header = Header()
joint_states.header.stamp = rospy.Time.now()
joint_states.name = ['virtual_x', 'virtual_y', 'virtual_theta']
joint_states.position = [self.X[0], self.X[1], self.X[2]]
joint_states.velocity = []
joint_states.effort = []
self.pub.publish(joint_states)
return
def default(self, ci='unused'):
js = JointState()
js.header = self.get_ros_header()
# collect name and positions of jointstates from sensor
js.name = list(self.data.keys())[1:]
js.position = list(self.data.values())[1:]
# for now leaving out velocity and effort
#js.velocity = [1, 1, 1, 1, 1, 1, 1]
#js.effort = [50, 50, 50, 50, 50, 50, 50]
self.publish(js)
def default(self, ci='unused'):
""" Publish the data of the posture sensor as a ROS JointState message """
js = JointState()
js.header = self.get_ros_header()
js.name = ['head_pan_joint', 'head_tilt_joint']
js.position = [
self.data['pan'],
self.data['tilt']
]
self.publish(js)
def default(self, ci='unused'):
""" Publish the data of the posture sensor as a ROS JointState message """
js = JointState()
js.header = self.get_ros_header()
# collect name and positions of jointstates from sensor
js.name = self.data.keys()
js.position = self.data.values()
# for now leaving out velocity and effort
#js.velocity = [1, 1, 1, 1, 1, 1, 1]
#js.effort = [50, 50, 50, 50, 50, 50, 50]
self.publish(js)
def get_ee_for_joints(self, joints):
if self.side_prefix == "l":
joint_names = self.gui.l_joint_names
else:
joint_names = self.gui.r_joint_names
joint_state = JointState()
joint_state.header = self.fk_request.header
joint_state.name = joint_names
joint_state.position = joints
multi_dof_joint_state = MultiDOFJointState()
# TODO: build it
return self.get_ee_for_state(joint_state, multi_dof_joint_state)
def publish_arm_state(self):
arm_msg = JointState()
arm_msg.header = Header()
arm_msg.header.stamp = rospy.Time.now()
arm_msg.name = ['base_to_lever_arm', 'lever_arm_to_digging_arm']
height_pos_inch = self.height_enc_to_inch(self.height_pos['m1'],self.h_params['m1'])
pitch_pos_inch = self.pitch_enc_to_inch(self.pitch_pos['m1'],self.p_params['m1'])
height_angle = math.radians(self.height_angle_offset) - self.height_dist_to_angle(height_pos_inch)
pitch_angle = self.pitch_dist_to_angle(pitch_pos_inch)
arm_msg.position = [height_angle, pitch_angle]
self.joint_pub.publish(arm_msg)
def talker():
global angles
pub = rospy.Publisher('joint_states', JointState, queue_size=10)
rate = rospy.Rate(10)
joint_states = JointState()
joint_states.header = Header()
while not rospy.is_shutdown():
joint_states.header.stamp = rospy.Time.now()
joint_states.name = ['servo1', 'servo2', 'servo3', 'servo4']
joint_states.position = angles
joint_states.velocity = []
joint_states.effort = []
pub.publish(joint_states)
print(joint_states)
rate.sleep()
def default(self, ci='unused'):
message = JointState()
message.header = self.get_ros_header()
message.name = [''] * 7
message.position = [0] * 7
message.velocity = [0] * 7
message.effort = [0] * 7
# Define name used to export joints
base_name = "kuka_joint_"
for i in range(7):
message.name[i] = base_name + ("%d" % (i+1) )
message.position[i] = self.data[ "kuka_" + ("%d" % (i+1) ) ]
self.publish(message)
def talker():
pub = rospy.Publisher('joint_states', JointState, queue_size=100)
rospy.init_node('joint_states_publisher')
rate = rospy.Rate(30) # 10hz
AFL, AFR, ABR, ABL, WFL, WFR, WBR, WBL, LIDAR = 0,0,0,0,0,0,0,0,0
rotAFL, rotAFR, rotABR, rotABL, rotWFL, rotWFR, rotWBR, rotWBL, rotLIDAR=0,0,0,0,0,0,0,0,0
while not rospy.is_shutdown():
hello_str = JointState()
hello_str.header = Header()
hello_str.header.stamp = rospy.Time.now()
hello_str.name = ['joint_axe_front_left', 'joint_axe_front_right', 'joint_axe_back_left', 'joint_axe_back_right', 'joint_wheel_front_left', 'joint_wheel_front_right', 'joint_wheel_back_left', 'joint_wheel_back_right', 'joint_LIDAR']
hello_str.position = [AFL, AFR, ABR, ABL, WFL, WFR, WBR, WBL, LIDAR]
hello_str.velocity = [rotAFL, rotAFR, rotABR, rotABL,rotWFL, rotWFR, rotWBR, rotWBL, rotLIDAR]
hello_str.effort = []
pub.publish(hello_str)
WFL, WFR, WBR, WBL= WFL+0.2, WFR+0.3, WBR+0.3, WBL+0.2
AFL, AFR, ABR, ABL, WFL, WFR, WBR, WBL, LIDAR, rotAFL, rotAFR, rotABR, rotABL, rotWFL, rotWFR, rotWBR, rotWBL, rotLIDAR=[i for i in check_radian([AFL, AFR, ABR, ABL, WFL, WFR, WBR, WBL, LIDAR])]
rate.sleep()
def main_fcn():
pub = rospy.Publisher('joint_states',JointState,queue_size = 10)
pub2 = rospy.Publisher('read_angles',Int32,queue_size = 10)
pub3 = rospy.Publisher('uarm_status',String,queue_size = 100)
rospy.init_node('display',anonymous = True)
rate = rospy.Rate(20)
joint_state_send = JointState()
joint_state_send.header = Header()
joint_state_send.name = ['base_to_base_rot','base_rot_to_link_1','link_1_to_link_2' ,'link_2_to_link_3','link_3_to_link_end']
joint_state_send.name = joint_state_send.name+ ['link_1_to_add_1','link_2_to_add_4','link_3_to_add_2','base_rot_to_add_3','link_2_to_add_5']
angle = {}
trigger = 1
while not rospy.is_shutdown():
joint_state_send.header.stamp = rospy.Time.now()
pub2.publish(1)
if trigger == 1:
pub3.publish("detach")
time.sleep(1)
trigger = 0
angle['s1'] = rospy.get_param('servo_1')*math.pi/180
angle['s2'] = rospy.get_param('servo_2')*math.pi/180
angle['s3'] = rospy.get_param('servo_3')*math.pi/180
angle['s4'] = rospy.get_param('servo_4')*math.pi/180
joint_state_send.position = [angle['s1'],angle['s2'],-angle['s2']-angle['s3'],angle['s3'],angle['s4']]
joint_state_send.position = joint_state_send.position + [-angle['s2']-angle['s3'],angle['s3'],PI/2-angle['s3']]
joint_state_send.position = joint_state_send.position + [angle['s2']-PI,angle['s2']+angle['s3']-PI/2]
joint_state_send.velocity = [0]
joint_state_send.effort = []
pub.publish(joint_state_send)
rate.sleep()
def callback(self, data):
num_joints = len(data.name)
if len(data.position) < num_joints:
rospy.logerr("Position array shorter than names %s < %d"%(len(data.position), num_joints))
return
elif len(data.velocity) < num_joints:
rospy.logerr("Velocity array shorter than names %s < %d"%(len(data.velocity), num_joints))
return
elif len(data.effort) < num_joints:
rospy.logerr("Effort array shorter than names %s < %d"%(len(data.effort), num_joints))
return
for i in xrange(0, num_joints):
self.observed_states[data.name[i]] = JS(data.name[i],
data.header,
data.position[i],
data.velocity[i],
data.effort[i])
todelete = [k for k, v in self.observed_states.iteritems() if data.header.stamp - v.header.stamp > rospy.Duration().from_sec(10.0) ] #hack parametersize
for td in todelete:
del self.observed_states[td]
# Only publish if there has been as many updates as there are joints, otherwise odom, gets zero deltas and the robot jerks around.
if self.updates_since_publish < len(self.observed_states.keys()):
self.updates_since_publish += 1
return
self.updates_since_publish = 0
msg_out = JointState()
msg_out.header = data.header
for k, v in self.observed_states.iteritems():
#print k, v
msg_out.name.append(v.name)
msg_out.position.append(v.position)
msg_out.velocity.append(v.velocity)
msg_out.effort.append(v.effort)
self.pub.publish(msg_out)
def loadFromYamlFile(self, filename):
f = open(filename, "r")
yamlData = yaml.load_all(f)
for singleData in yamlData:
jointState = JointState()
for k,v in singleData.items():
#print k, "->", v
#print "\n",
if "header" == k:
jointState.header = v
elif "name" == k:
jointState.name = v
elif "effort" == k:
jointState.effort = v
elif "velocity" == k:
jointState.velocity = v
elif "position" == k:
jointState.position = v
#print jointState
self.jointStates.append(jointState)
f.close()
def default(self, ci='unused'):
js = JointState()
js.header = self.get_ros_header()
js.name = [
'kuka_arm_0_joint', 'kuka_arm_1_joint', 'kuka_arm_2_joint',
'kuka_arm_3_joint', 'kuka_arm_4_joint', 'kuka_arm_5_joint',
'kuka_arm_6_joint', 'head_pan_joint', 'head_tilt_joint'
]
js.position = [
self.data['seg0'],
self.data['seg1'],
self.data['seg2'],
self.data['seg3'],
self.data['seg4'],
self.data['seg5'],
self.data['seg6'],
self.data['pan'],
self.data['tilt']
]
#js.velocity = [1, 1, 1, 1, 1, 1, 1]
#js.effort = [50, 50, 50, 50, 50, 50, 50]
self.publish(js)
def talker():
pub = rospy.Publisher('joint_states', JointState, queue_size=10)
rospy.init_node('sidewinder_uart_communicator')
rospy.Subscriber('cmd_vel', Twist, callback)
rate = rospy.Rate(100) # 100 hz
joint_state_msg = JointState()
joint_state_msg.header = Header()
joint_state_msg.name = ['joint1', 'joint2', 'joint3']
joint_state_msg.velocity = []
joint_state_msg.effort = []
while not rospy.is_shutdown():
ser_line = None
while ser_line is None or ser_line[0] != '2':
ser_line = ser.readline()
joint_states = ser_line.split(' ')[1:]
#joint_states = [joint_states[1], joint_states[3], joint_states[5]]
#rospy.logerr(joint_states)
joint_state_msg.header.stamp = rospy.Time.now()
joint_state_msg.position = [float(i) * math.pi / 180.0 for i in joint_states]
pub.publish(joint_state_msg)
rate.sleep()
def DataRecord():
rospy.init_node('data_record', anonymous=True)
r = rospy.Rate(10) #change this to any feasible rate
# start the data record
HandPublisher = rospy.Publisher('/bhand_node/command', JointState, queue_size=1)
ArmPublisher = rospy.Publisher('/KUKA/joint_imp_cmd', JointStateImpedance, queue_size=1)
rospy.Subscriber('/usr/cmd',String, GetUsrCmd, queue_size=1)
global UsrCmd
UsrCmd = String()
rospy.Subscriber('/joint_states', JointState, GetJoints, queue_size=1)
global HandJointsCurrent
HandJointsCurrent = np.zeros([4])
# The hand start with fully open and close fingers gradually.
K_range= 1.4 #1.5~1.6
HandJointsFinal = K_range* np.array([1.0,1.0,1.0,0.4])
SpeedScale = 0.0
HandSpeed = SpeedScale * np.array([0.1, 0.1, 0.1, 0.0])
HandCmd = JointState()
h = Header()
h.stamp = rospy.Time.now()
HandCmd.header = h
HandCmd.name = ['bh_j23_joint', 'bh_j12_joint', 'bh_j22_joint', 'bh_j32_joint', 'bh_j33_joint', 'bh_j13_joint', 'bh_j11_joint', 'bh_j21_joint']
# HandCmd.position = [HandJoints[0], 0.0, HandJoints[1], 0.0, HandJoints[2], 0.0, HandJoints[3], HandJoints[3]]
# HandCmd.velocity= [HandSpeed[0], 0.0, HandSpeed[1], 0.0, HandSpeed[2], 0.0, 0.0, 0.0]
HandCmd.effort = ''
# The arm is in the gravity compensation mode
ArmJointImpCmd = JointStateImpedance()
ArmJointImpCmd.name = 'LWR'
ArmJointImpCmd.position = ''
ArmJointImpCmd.velocity = ''
ArmJointImpCmd.effort = ''
ArmJointImpCmd.stiffness = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
Delta_time = 0.1
HandJoints = np.zeros([4])
bMove = True
bArm = True
rospy.sleep(.1)
while not rospy.is_shutdown():
h.stamp = rospy.Time.now()
HandCmd.header = h
# print UsrCmd.data
# print UsrCmd.data == 'stop'
if UsrCmd.data == 'arm' or bArm:
ArmPublisher.publish(ArmJointImpCmd)
else:
print 'Arm is in position mode'
if UsrCmd.data != 'stop' and LA.norm(HandJointsCurrent-HandJointsFinal)>0.1 and bMove:
# HandJoints = HandJoints + HandSpeed * Delta_time
# bMove = False
print 'run once'
HandJoints = HandJointsCurrent + (HandJointsFinal-HandJointsCurrent)/LA.norm(HandJointsFinal-HandJointsCurrent)*0.1
# print HandJoints
# print HandJointsCurrent
# HandJoints= HandJointsFinal
HandCmd.position = [0.0, HandJoints[0], HandJoints[1],HandJoints[2], 0.0, 0.0, HandJoints[3], HandJoints[3]]
HandCmd.velocity= [0.0, HandSpeed[0], HandSpeed[1], HandSpeed[2], 0.0, 0.0, HandSpeed[3], HandSpeed[3]]
HandPublisher.publish(HandCmd)
else:
print "stop command the hand"
r.sleep()
def main(ra,max_vel12,max_vel3):
# Create Pose Publisher and Subscriber
state_pub=rospy.Publisher('/dvrk/PSM1/set_robot_state',String,queue_size=10)
joint_pub= rospy.Publisher('/dvrk/PSM1/set_position_joint', JointState, queue_size=10)
rospy.Subscriber('/dvrk/PSM1/state_joint_current', JointState, callback_joint)
rospy.init_node('Talkity_talk_talk',anonymous=True)
rate = rospy.Rate(ra)
rospy.sleep(1)
#set robot state
state_pub.publish("DVRK_POSITION_JOINT")
rospy.sleep(1)
#hardcode home to zero
joint_msg= JointState()
joint_msg.header = Header()
joint_msg.header.stamp = rospy.Time.now()
joint_msg.name = ['outer_yaw', 'outer_pitch', 'outer_insertion', 'outer_roll', 'outer__wrist_pitch','outer_wrist_jaw', 'jaw']
joint_msg.position=[0]*7
joint_msg.velocity = []
joint_msg.effort = []
rospy.sleep(1)
scale=0.6
#d12=max_vel12/ra*scale
#d3=max_vel3/ra*scale
d=0.008
thresh=0.008
dq1=thresh+1
dq2=thresh+1
dq3=thresh+1
while not rospy.is_shutdown():
if -thresh<dq1<thresh and -thresh<dq2<thresh and -thresh<dq3<thresh:
joint_msg.position=[]
print(joint_msg)
joint_pub.publish(joint_msg)
break
else:
if -thresh<joint_sub.position[0]<thresh:
dq1=joint_sub.position[0]
elif joint_sub.position[0]<0:
dq1=joint_sub.position[0]+d
else:
dq1=joint_sub.position[0]-d
if -thresh<joint_sub.position[1]<thresh:
dq2=joint_sub.position[1]
elif joint_sub.position[1]<0:
dq2=joint_sub.position[1]+d
else:
dq2=joint_sub.position[1]-d
if -thresh<joint_sub.position[2]<thresh:
dq3=joint_sub.position[2]
elif joint_sub.position[2]<0:
dq3=joint_sub.position[2]+d/5
else:
dq3=joint_sub.position[2]-d/5
#joint_msg.position = [dq1, dq2 , dq3 , None, None, None, None]
joint_msg.position[0]=dq1
joint_msg.position[1]=dq2
joint_msg.position[2]=dq3
joint_msg.position[3]=joint_sub.position[3]
joint_msg.position[3]=joint_sub.position[4]
joint_msg.position[3]=joint_sub.position[5]
joint_msg.position[3]=joint_sub.position[6]
print(joint_msg)
joint_pub.publish(joint_msg)
rospy.sleep(1/float(ra))
