Robotics

1#!/usr/bin/env python3
2
3import rclpy
4import rclpy.node
5from geometry_msgs.msg import PoseWithCovarianceStamped
6import rclpy.qos
7from rclpy.executors import MultiThreadedExecutor
8
9import pydrake
10from pydrake.all import (VectorSystem, DiagramBuilder, SymbolicVectorSystem, LogVectorOutput,
11                         Variable, Simulator, cos, sin)
12import numpy
13
14L = 0.6
15K_us = 1.0
16g = 9.81
17# you will write the control law in it in the next cell
18# by defining the function called "lyapunov_controller"
19
20
21class PointTrackingController(VectorSystem):
22
23    def __init__(self):
24        # 3 inputs (robot state)
25        # 2 outputs (robot inputs)
26        VectorSystem.__init__(self, 3, 2)
27
28    def euclidean_dist(self, x, y, x_obs, y_obs):
29        # x and y are vehicle pose
30        return numpy.sqrt((x-x_obs)**2) + ((y-y_obs)**2)
31
32    def clip(self, val, min_, max_):
33        return min_ if val < min_ else max_ if val > max_ else val
34
35    def lyapunov_controller(self, f_x, f_y, theta, f_theta, theta_d):
36        k1 = 1E-4
37        k2 = 1E-1
38        u1 = k1 * numpy.copysign(1, (f_x*cos(theta) + f_y *
39                                     sin(theta))) * (f_x**2 + f_y ** 2 + f_theta**2)
40
41        p = u1*(f_x * cos(theta) + f_y * sin(theta)) + \
42            k2 * (theta_d - theta) * f_theta
43        if p >= 0.0:
44            u2 = k2 * (theta_d - theta) * ((L + K_us/g * u1**2)/u1)
45        else:
46            u2 = -u1 / f_theta * (f_x * cos(theta) + f_y *
47                                  sin(theta)) * ((L + K_us/g * u1**2)/u1)
48
49        return -self.clip(u1, -1.5, 1.5), -self.clip(u2, -0.6, 0.6)
50
51    def DoCalcVectorOutput(
52        self,
53        context,           # not used
54        cartesian_state,   # input of the controller
55        controller_state,  # not used
56        input              # output of the controller
57    ):
58
59        kLAMBDA = 0.0
60        kK = 0.05
61        kPULL = 10
62
63        # upack state of the robot x,y,theta
64        x, y, theta = cartesian_state
65
66        target_x, target_y = 0, 0
67        dist = self.euclidean_dist(x, y, target_x, target_y)
68        beta = kPULL*dist
69
70        if x >= 0.0:
71            f_x = 3 * (x**2) * (beta**(1.0/kK)) + (y**2)*(beta**(1.0/kK)) + kLAMBDA * (theta**2) * (beta**(1.0/kK)) + \
72                (x*(x - target_x)*(x**2 + y**2 + kLAMBDA*(theta**2)) * (beta **
73                 ((-kK+1.0)/kK))) / kK * numpy.sqrt((x-target_x)**2 + (y - target_y)**2)
74
75            f_y = x * (((y - target_y)*(x**2 + y**2 + kLAMBDA*(theta**2)) * (beta**((-kK+1.0)/kK))
76                        ) / kK * numpy.sqrt((x-target_x)**2 + (y - target_y)**2) + 2 * y * (beta**(1.0/kK)))
77            f_theta = 2 * kLAMBDA * theta * (beta**(1.0/kK))
78        else:
79            f_x = -3 * (x**2) * (beta**(1/kK)) - (y**2)*(beta**(1/kK)) - kLAMBDA * (theta**2) * (beta**(1.0/kK)) - \
80                (x*(x - target_x)*(x**2 + y**2 + kLAMBDA*(theta**2)) * (beta **
81                 ((-kK+1.0)/kK))) / kK * numpy.sqrt((x-target_x)**2 + (y - target_y)**2)
82
83            f_y = -x * (((y - target_y)*(x**2 + y**2 + kLAMBDA*(theta**2)) * (beta**((-kK+1)/kK))) /
84                        kK * numpy.sqrt((x-target_x)**2 + (y - target_y)**2) + 2 * y * (beta**(1.0/kK)))
85            f_theta = - 2 * kLAMBDA * theta * (beta**(1.0/kK))
86
87        theta_d = numpy.arctan2(-numpy.copysign(1, x)*f_y,
88                                -numpy.copysign(1, x)*f_x)
89
90        # evaluate the function below and return the robot's input
91        input[:] = self.lyapunov_controller(f_x, f_y, theta, f_theta, theta_d)
92
93
94class RosNode(rclpy.node.Node):
95    def __init__(self, *args):
96        super(RosNode, self).__init__("rosnode")
97        self.get_logger().info("Starting rosnode")
98
99        self.create_subscription(
100            PoseWithCovarianceStamped, "/vox_nav/cupoch/icp_base_to_map_pose",
101            self.robot_pose_callback, rclpy.qos.qos_profile_sensor_data)
102
103    def robot_pose_callback(self, msg):
104        self.get_logger().info("recieved pose %s" % str(msg.pose.pose))
105
106
107def main():
108
109    print("Drake is installed under: ")
110    print(pydrake.getDrakePath())
111
112    x = Variable("x")
113    y = Variable("y")
114    theta = Variable("theta")        # vehicle orienttaion
115    cartesian_state = [x, y, theta]
116    v = Variable("v")                # driving velocity input
117    delta = Variable("delta")        # steering velocity input
118    input = [v, delta]
119
120    # nonlinear dynamics, the whole state is measured (output = state)
121    dynamics = [v * cos(theta),
122                v * sin(theta),
123                v * numpy.tan(delta)/L]
124
125    robot = SymbolicVectorSystem(
126        state=cartesian_state,
127        input=input,
128        output=cartesian_state,
129        dynamics=dynamics,
130    )
131
132    # construction site for our closed-loop system
133    builder = DiagramBuilder()
134
135    # add the robot to the diagram
136    # the method .AddSystem() simply returns a pointer to the system
137    # we passed as input, so it's ok to give it the same name
138    robot = builder.AddSystem(robot)
139
140    # add the controller
141    controller = builder.AddSystem(PointTrackingController())
142
143    # wire the controller with the system
144    builder.Connect(robot.get_output_port(0), controller.get_input_port(0))
145    builder.Connect(controller.get_output_port(0), robot.get_input_port(0))
146
147    # add a logger to the diagram
148    # this will store the state trajectory
149    logger = LogVectorOutput(robot.get_output_port(0), builder)
150
151    # complete the construction of the diagram
152    diagram = builder.Build()
153
154    # set up a simulation environment
155    simulator = Simulator(diagram)
156
157    # set the initial cartesian state to a random initial position
158    # try initial_state = np.random.randn(3) for a random initial state
159    initial_state = [10, 16, 1.57]
160    context = simulator.get_mutable_context()
161    context.SetContinuousState(initial_state)
162
163    # simulate from zero to sim_time
164    # the trajectory will be stored in the logger
165    integrator = simulator.get_mutable_integrator()
166    target_accuracy = 1E-4
167    integrator.set_target_accuracy(target_accuracy)
168    maximum_step_size = 0.1
169    integrator.set_maximum_step_size(maximum_step_size)
170    minimum_step_size = 2E-5
171    integrator.set_requested_minimum_step_size(minimum_step_size)
172    integrator.set_throw_on_minimum_step_size_violation(True)
173    integrator.set_fixed_step_mode(True)
174    simulator.set_target_realtime_rate(1.0)
175
176    rclpy.init()
177    node = RosNode()
178    executor = MultiThreadedExecutor()
179    executor.add_node(node)
180
181    while True:
182        executor.spin_once(0.05)
183        simulator.AdvanceTo(context.get_time() + 0.1)
184        node.get_logger().info("Another step, time is: %s" % str(context.get_time()))
185
186
187if __name__ == '__main__':
188    main()
189

1let arr1 = [
2  { key: "WIHUGDYVWJ", className: "science" },
3  { key: "qwljdhkuqwdnqwdk", className: "english" },
4  { key: "likubqwd", className: "robotics" }
5];
6

1let arr1 = [
2  { key: "WIHUGDYVWJ", className: "science" },
3  { key: "qwljdhkuqwdnqwdk", className: "english" },
4  { key: "likubqwd", className: "robotics" }
5];
6let arr2 = [
7  { key: "WIHUGDYVWJ", title: "math" },
8  { key: "qwljdhkuqwdnqwdk", title: "english" },
9  { key: "likubqwd", title: "robotics" }
10];
11

1[external system]-https://example.com:5000/5001----nginx----https://example.com:5000
2                                                        \---http://example.com:5001
3
4http://example.com:5000 or 5001 redirects to https.
5

1sudo python3 /home/ubuntu/Robotics/QuadrupedRobot/StanfordQuadruped/calibrate_tool.py
2

1sudo python3 /home/ubuntu/Robotics/QuadrupedRobot/StanfordQuadruped/calibrate_tool.py
2python3 /home/ubuntu/Robotics/QuadrupedRobot/StanfordQuadruped/calibrate_tool.py
3

1SELECT 
2    CASE 
3        WHEN PATINDEX('%[0-9 ]%', job_title) > 0 
4            THEN RTRIM(SUBSTRING(Job_title, 1, PATINDEX('%[0-9 ]%', job_title) - 1))
5            ELSE JOB_TITLE
6    END
7FROM
8    my_table
9WHERE 
10    PATINDEX('%[0-9]%', JOB_TITLE) <> 0
11

1Uncaught DOMException: Can't create RTCPeerConnections when the network is down
2

1Uncaught DOMException: Can't create RTCPeerConnections when the network is down
2      iceServers: [{
3        urls: ["stun:stun.l.google.com:19302"]
4      }]
5

1Starting >>> geometric_shapes
2--- stderr: geometric_shapes
3CMake Error at CMakeLists.txt:47 (find_package):
4  By not providing "Findeigen3_cmake_module.cmake" in CMAKE_MODULE_PATH this
5  project has asked CMake to find a package configuration file provided by
6  "eigen3_cmake_module", but CMake did not find one.
7
8  Could not find a package configuration file provided by
9  "eigen3_cmake_module" with any of the following names:
10
11    eigen3_cmake_moduleConfig.cmake
12    eigen3_cmake_module-config.cmake
13
14  Add the installation prefix of "eigen3_cmake_module" to CMAKE_PREFIX_PATH
15  or set "eigen3_cmake_module_DIR" to a directory containing one of the above
16  files.  If "eigen3_cmake_module" provides a separate development package or
17  SDK, be sure it has been installed.
18
19
20---
21Failed   <<< geometric_shapes [1.20s, exited with code 1]
22
23Summary: 0 packages finished [1.46s]
24  1 package failed: geometric_shapes
25  1 package had stderr output: geometric_shapes
26  33 packages not processed
27

1<!DOCTYPE html>
2<html>
3
4<head>
5    <meta charset="utf-8" />
6    <title>Personal Website</title>
7    <style>
8        body{
9            background-color: #97BFB4;
10        }
11
12        hr {
13            background-color: whitesmoke;
14            border-style: dotted;
15            height : 0px;
16            width: 10%;
17        }
18    </style>
19</head>
20
21<body>
22    <table cellspacing= "20">
23        <tbody>
24            <tr>
25                <td><img src=""></td>
26
27                <td>
28                    <h1> <a href="">Personal
29                        Site </a></h1>
30                    <p><em> Computer Engineer at Stony Brook University </em></p>
31                
32                    <p>
33                        
34                    </p>
35                </td>
36            </tr>
37        </tbody>
38    </table>
39    <hr>
40    <h3><a href="">Experience</a></h3>
41    <table>
42        <thead>
43            <th>
44                Dates
45            </th>
46            <th>
47                Work
48            </th>
49        </thead>
50        <tbody>
51            <tr>
52                <td>June 2021</td>
53                <td>Undergraduate Researcher</td>
54            </tr>
55            <tr>
56                <td>Sept 2020</td>
57                <td>Vice President Of Robotics</td>
58            </tr>
59        </tbody>
60       <tfoot></tfoot>
61        
62    </table>
63    
64    <hr>
65</body>
66

1<!DOCTYPE html>
2<html>
3
4<head>
5    <meta charset="utf-8" />
6    <title>Personal Website</title>
7    <style>
8        body{
9            background-color: #97BFB4;
10        }
11
12        hr {
13            background-color: whitesmoke;
14            border-style: dotted;
15            height : 0px;
16            width: 10%;
17        }
18    </style>
19</head>
20
21<body>
22    <table cellspacing= "20">
23        <tbody>
24            <tr>
25                <td><img src=""></td>
26
27                <td>
28                    <h1> <a href="">Personal
29                        Site </a></h1>
30                    <p><em> Computer Engineer at Stony Brook University </em></p>
31                
32                    <p>
33                        
34                    </p>
35                </td>
36            </tr>
37        </tbody>
38    </table>
39    <hr>
40    <h3><a href="">Experience</a></h3>
41    <table>
42        <thead>
43            <th>
44                Dates
45            </th>
46            <th>
47                Work
48            </th>
49        </thead>
50        <tbody>
51            <tr>
52                <td>June 2021</td>
53                <td>Undergraduate Researcher</td>
54            </tr>
55            <tr>
56                <td>Sept 2020</td>
57                <td>Vice President Of Robotics</td>
58            </tr>
59        </tbody>
60       <tfoot></tfoot>
61        
62    </table>
63    
64    <hr>
65</body>
66<!DOCTYPE html>
67<html>
68
69<head>
70    <meta charset="utf-8" />
71    <title>Personal Website</title>
72    <style>
73        body{
74            background-color: #97BFB4;
75        }
76
77        hr {
78            background-color: whitesmoke;
79            border-style: dotted;
80            border-width: 2px 0 0 0;
81            height : 0px;
82            width: 10%;
83        }
84    </style>
85</head>
86
87<body>
88    <table cellspacing= "20">
89        <tbody>
90            <tr>
91                <td><img src=""></td>
92
93                <td>
94                    <h1> <a href="">Personal
95                        Site </a></h1>
96                    <p><em> Computer Engineer at Stony Brook University </em></p>
97
98                    <p>
99
100                    </p>
101                </td>
102            </tr>
103        </tbody>
104    </table>
105    <hr>
106    <h3><a href="">Experience</a></h3>
107    <table>
108        <thead>
109            <th>
110                Dates
111            </th>
112            <th>
113                Work
114            </th>
115        </thead>
116        <tbody>
117            <tr>
118                <td>June 2021</td>
119                <td>Undergraduate Researcher</td>
120            </tr>
121            <tr>
122                <td>Sept 2020</td>
123                <td>Vice President Of Robotics</td>
124            </tr>
125        </tbody>
126       <tfoot></tfoot>
127
128    </table>
129
130    <hr>
131</body>

1<!DOCTYPE html>
2<html>
3
4<head>
5    <meta charset="utf-8" />
6    <title>Personal Website</title>
7    <style>
8        body{
9            background-color: #97BFB4;
10        }
11
12        hr {
13            background-color: whitesmoke;
14            border-style: dotted;
15            height : 0px;
16            width: 10%;
17        }
18    </style>
19</head>
20
21<body>
22    <table cellspacing= "20">
23        <tbody>
24            <tr>
25                <td><img src=""></td>
26
27                <td>
28                    <h1> <a href="">Personal
29                        Site </a></h1>
30                    <p><em> Computer Engineer at Stony Brook University </em></p>
31                
32                    <p>
33                        
34                    </p>
35                </td>
36            </tr>
37        </tbody>
38    </table>
39    <hr>
40    <h3><a href="">Experience</a></h3>
41    <table>
42        <thead>
43            <th>
44                Dates
45            </th>
46            <th>
47                Work
48            </th>
49        </thead>
50        <tbody>
51            <tr>
52                <td>June 2021</td>
53                <td>Undergraduate Researcher</td>
54            </tr>
55            <tr>
56                <td>Sept 2020</td>
57                <td>Vice President Of Robotics</td>
58            </tr>
59        </tbody>
60       <tfoot></tfoot>
61        
62    </table>
63    
64    <hr>
65</body>
66<!DOCTYPE html>
67<html>
68
69<head>
70    <meta charset="utf-8" />
71    <title>Personal Website</title>
72    <style>
73        body{
74            background-color: #97BFB4;
75        }
76
77        hr {
78            background-color: whitesmoke;
79            border-style: dotted;
80            border-width: 2px 0 0 0;
81            height : 0px;
82            width: 10%;
83        }
84    </style>
85</head>
86
87<body>
88    <table cellspacing= "20">
89        <tbody>
90            <tr>
91                <td><img src=""></td>
92
93                <td>
94                    <h1> <a href="">Personal
95                        Site </a></h1>
96                    <p><em> Computer Engineer at Stony Brook University </em></p>
97
98                    <p>
99
100                    </p>
101                </td>
102            </tr>
103        </tbody>
104    </table>
105    <hr>
106    <h3><a href="">Experience</a></h3>
107    <table>
108        <thead>
109            <th>
110                Dates
111            </th>
112            <th>
113                Work
114            </th>
115        </thead>
116        <tbody>
117            <tr>
118                <td>June 2021</td>
119                <td>Undergraduate Researcher</td>
120            </tr>
121            <tr>
122                <td>Sept 2020</td>
123                <td>Vice President Of Robotics</td>
124            </tr>
125        </tbody>
126       <tfoot></tfoot>
127
128    </table>
129
130    <hr>
131</body><!DOCTYPE html>
132<html>
133
134<head>
135    <meta charset="utf-8" />
136    <title>Personal Website</title>
137    <style>
138        body{
139            background-color: #97BFB4;
140        }
141
142        hr {
143            /* background-color: whitesmoke; */
144            border-style: dotted none none none;
145            border-color: grey;
146            border-width: 5px;
147            height : 0px;
148            width: 10%;
149        }
150    </style>
151</head>
152
153<body>
154    <table cellspacing= "20">
155        <tbody>
156            <tr>
157                <td><img src=""></td>
158
159                <td>
160                    <h1> <a href="">Personal
161                        Site </a></h1>
162                    <p><em> Computer Engineer at Stony Brook University </em></p>
163
164                    <p>
165
166                    </p>
167                </td>
168            </tr>
169        </tbody>
170    </table>
171    <hr>
172    <h3><a href="">Experience</a></h3>
173    <table>
174        <thead>
175            <th>
176                Dates
177            </th>
178            <th>
179                Work
180            </th>
181        </thead>
182        <tbody>
183            <tr>
184                <td>June 2021</td>
185                <td>Undergraduate Researcher</td>
186            </tr>
187            <tr>
188                <td>Sept 2020</td>
189                <td>Vice President Of Robotics</td>
190            </tr>
191        </tbody>
192       <tfoot></tfoot>
193
194    </table>
195
196    <hr>
197</body>

1package com.example.hello_pepper_2
2
3import android.os.Bundle
4import android.util.Log
5import android.com.aldebaran.qi.sdk.QiContext
6import android.com.aldebaran.qi.sdk.QiSDK
7import android.com.aldebaran.qi.sdk.RobotLifecycleCallbacks
8import android.com.aldebaran.qi.sdk.`object`.conversation.Say
9import android.com.aldebaran.qi.sdk.builder.SayBuilder
10import android.com.aldebaran.qi.sdk.design.activity.RobotActivity
11
12class MainActivity : RobotActivity(), RobotLifecycleCallbacks {
13
14    var robotContext: QiContext? = null
15
16    override fun onCreate(savedInstanceState: Bundle?) {
17        super.onCreate(savedInstanceState)
18        // Register the RobotLifecycleCallbacks to this Activity.
19        QiSDK.register(this, this)
20        setContentView(R.layout.activity_main)
21
22        Log.e("MSG", "onCreate")
23        Log.e("Context", "robot.Context.toString()")
24    }
25
26    override fun onStart() {
27        super.onStart()
28
29        Log.e("MSG", "onStart")
30        Log.e("Context", "robot.Context.toString()")
31    }
32
33    override fun onResume() {
34        super.onResume()
35
36        Log.e("MSG", "onResume")
37        Log.e("Context", "robot.Context.toString()")
38    }
39
40    override fun onRestart() {
41        super.onRestart()
42
43        Log.e("MSG", "onRestart")
44        Log.e("Context", "robot.Context.toString()")
45    }
46
47    override fun onPause() {
48        super.onPause()
49
50        Log.e("MSG", "onPause")
51        Log.e("Context", "robot.Context.toString()")
52    }
53
54    override fun onStop() {
55        super.onStop()
56
57        Log.e("MSG", "onStop")
58        Log.e("Context", "robot.Context.toString()")
59    }
60
61    override fun onDestroy() {
62        // Unregister the RobotLifecycleCallbacks for this Activity.
63        QiSDK.unregister(this, this)
64        super.onDestroy()
65
66        robotContext = null
67
68        Log.e("MSG", "onDestroy")
69        Log.e("Context", "robot.Context.toString()")
70    }
71
72    override fun onRobotFocusGained(qiContext: QiContext) {
73        // The robot focus is gained.
74
75        robotContext = qiContext
76
77        saySomething("Hello Humans")
78
79        Log.e("MSG", "onRobotFocusGained")
80        Log.e("Context", "robot.Context.toString()")
81    }
82
83    override fun onRobotFocusLost() {
84        // The robot focus is lost.
85
86        Log.e("MSG", "onRobotFocusLost")
87        Log.e("Context", "robot.Context.toString()")
88    }
89
90    override fun onRobotFocusRefused(reason: String) {
91        // The robot focus is refused.
92
93        Log.e("MSG", "onRobotFocusRefused")
94        Log.e("Context", "robot.Context.toString()")
95    }
96
97    fun saySomething(phrase: String) {
98        Log.e("MSG", "saySomething")
99        Log.e("Context", "robot.Context.toString()")
100
101        val say: Say = SayBuilder
102            .with(robotContext)
103            .withText(phrase)
104            .build()
105
106        say.run()
107    }
108}
109
110