<commit_before># -*- coding: utf-8 -*-
"""
Created on Thu Jul  2 12:52:50 2015

This class

@author: skylion
"""
import trollius #NOTE: Trollius requires protobuffer from Google
from trollius import From

import pygazebo
import pygazebo.msg.joint_cmd_pb2
import time 
import popupcad
from lxml import etree
import subprocess
import os

try:
    import itertools.izip as zip
except ImportError:
    pass

def apply_joint_forces(world_name, robot_name, joint_names, forces, duration=0):
    """ Applies joint forces via Trollius over a specified duration.
    """    
    wait_net_service('localhost',11345)    
    print("Net serviced detected. Proceeding")

    @trollius.coroutine
    def joint_force_loop(world_name, robot_name, joint_name, force, duration):
        manager = yield From(pygazebo.connect())
        print("connected")
        
        
        publisher = yield From(
            manager.advertise('/gazebo/' + world_name + '/' + robot_name + '/joint_cmd',
                              'gazebo.msgs.JointCmd'))
    
        message = pygazebo.msg.joint_cmd_pb2.JointCmd()
        message.name = robot_name + '::' + joint_name #format should be: name_of_robot + '::name_of_joint'
        message.force = force
        
        t_end = time.time() + duration # The time that you want the controller to stop
        while time.time() < t_end or duration == -1:
            try:
                yield From(publisher.publish(message))
                yield From(trollius.sleep(1.0))
            except:
                pass
                break
        print("Connection closed")
        
    def loop_in_thread(loop, tasks):
        trollius.set_event_loop(loop)
        loop.run_until_complete(trollius.wait(tasks))
        
    tasks = []
    for joint_name, force in zip(joint_names, forces):
        tasks.append(trollius.Task(joint_force_loop(world_name, robot_name, joint_name, force, duration)))
    
    
    loop = trollius.get_event_loop()    
    
    
    #Experimental code to make this loop non-blocking
    #loop = trollius.get_event_loop()
    #loop_thread_tasks = lambda t_loop: loop_in_thread(t_loop, tasks)
    #import threading
    #t = threading.Thread(target=loop_thread_tasks, args=(loop,))
    #t.start()
    #return t
    loop.run_until_complete(trollius.wait(tasks))
    
#Applies the joint using PyGazebo. All joints are applied concurrently.
#Allows for granular PID control
#TODO Allow users to specify PID variables
def apply_joint_pos(world_name, robot_name, joint_names, poses, duration=-1):
    """ Applies a joint position asynchronously over multiple joint members, 
        then waits for a specified duration if specified.    
    """    
    wait_net_service('localhost', 11345)    
    print("Net serviced detected. Proceeding")
    
    @trollius.coroutine
    def joint_pose_loop(world_name, robot_name, joint_name, pose, duration):
        manager = yield From(pygazebo.connect())
        print("connected")
        
        
        publisher = yield From(
            manager.advertise('/gazebo/' + world_name + '/' + robot_name + '/joint_cmd',
                              'gazebo.msgs.JointCmd'))
    
        message = pygazebo.msg.joint_cmd_pb2.JointCmd()
        message.name = robot_name + '::' + joint_name #format should be: name_of_robot + '::name_of_joint'<commit_msg>Fixed minor bug in Trollius control<commit_after># -*- coding: utf-8 -*-
"""
Created on Thu Jul  2 12:52:50 2015

This class

@author: skylion
"""
import trollius #NOTE: Trollius requires protobuffer from Google
from trollius import From

import pygazebo
import pygazebo.msg.joint_cmd_pb2
import time 
import popupcad
from lxml import etree
import subprocess
import os

try:
    import itertools.izip as zip
except ImportError:
    pass

def apply_joint_forces(world_name, robot_name, joint_names, forces, duration=0):
    """ Applies joint forces via Trollius over a specified duration.
    """    
    wait_net_service('localhost',11345)    
    print("Net serviced detected. Proceeding")

    @trollius.coroutine
    def joint_force_loop(world_name, robot_name, joint_name, force, duration):
        manager = yield From(pygazebo.connect())
        print("connected")
        
        
        publisher = yield From(
            manager.advertise('/gazebo/' + world_name + '/' + robot_name + '/joint_cmd',
                              'gazebo.msgs.JointCmd'))
    
        message = pygazebo.msg.joint_cmd_pb2.JointCmd()
        message.name = robot_name + '::' + joint_name #format should be: name_of_robot + '::name_of_joint'
        message.force = force
        
        t_end = time.time() + duration # The time that you want the controller to stop
        while time.time() < t_end or duration == -1:
            try:
                yield From(publisher.publish(message))
                yield From(trollius.sleep(1.0))
            except:
                pass
                break
        print("Connection closed")
        
    def loop_in_thread(loop, tasks):
        trollius.set_event_loop(loop)
        loop.run_until_complete(trollius.wait(tasks))
        
    tasks = []
    for joint_name, force in zip(joint_names, forces):
        tasks.append(trollius.Task(joint_force_loop(world_name, robot_name, joint_name, force, duration)))
    
    
    loop = trollius.get_event_loop()    
    
    
    #Experimental code to make this loop non-blocking
    #loop = trollius.get_event_loop()
    #loop_thread_tasks = lambda t_loop: loop_in_thread(t_loop, tasks)
    #import threading
    #t = threading.Thread(target=loop_thread_tasks, args=(loop,))
    #t.start()
    #return t
    loop.run_until_complete(trollius.wait(tasks))
    
#Applies the joint using PyGazebo. All joints are applied concurrently.
#Allows for granular PID control
#TODO Allow users to specify PID variables
def apply_joint_pos(world_name, robot_name, joint_names, poses, duration=0):
    """ Applies a joint position asynchronously over multiple joint members, 
        then waits for a specified duration if specified.    
    """    
    wait_net_service('localhost', 11345)    
    print("Net serviced detected. Proceeding")
    
    @trollius.coroutine
    def joint_pose_loop(world_name, robot_name, joint_name, pose, duration):
        manager = yield From(pygazebo.connect())
        print("connected")
        
        
        publisher = yield From(
            manager.advertise('/gazebo/' + world_name + '/' + robot_name + '/joint_cmd',
                              'gazebo.msgs.JointCmd'))
    
        message = pygazebo.msg.joint_cmd_pb2.JointCmd()
        message.name = robot_name + '::' + joint_name #format should be: name_of_robot + '::name_of_joint'
{'fraction_non_alphanumeric': 0.09595582748843456, 'fraction_numerical': 0.008953887479480675, 'mean_word_length': 2.6642974302897757, 'pattern_counts': {'":': 0, '<': 5, '<?xml version=': 0, '>': 3, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 2}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 0, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '5433234', 'n_tokens_mistral': 2120, 'n_tokens_neox': 2041, 'n_words': 506}